{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 42,
   "metadata": {},
   "outputs": [],
   "source": [
    "%matplotlib qt5\n",
    "import pandas as pd\n",
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "import seaborn as sns\n",
    "import time\n",
    "from PIL import Image\n",
    "from PIL import ImageDraw\n",
    "plt.style.use({'figure.figsize':(10, 10)})"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 43,
   "metadata": {},
   "outputs": [],
   "source": [
    "Velocity_tripod=0.289*40\n",
    "Velocity_quadruped=0.115*40\n",
    "Velocity_onebyone=0.096*40\n",
    "Up_D=np.array([-40,-20,0,20,40])\n",
    "Left_D=np.array([-60,-80,-100,-120])\n",
    "Right_D=np.array([60,80,100,120])\n",
    "Robot_R=40"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 44,
   "metadata": {},
   "outputs": [],
   "source": [
    "Q_table1_gait_state=np.array(['L0R0U0','L0R0U1','L0R0U2','L0R0U3',\n",
    "                'L0R1U0','L0R1U1','L0R1U2','L0R1U3',\n",
    "                'L0R2U0','L0R2U1','L0R2U2','L0R2U3',\n",
    "                'L0R3U0','L0R3U1','L0R3U2','L0R3U3',\n",
    "                'L1R0U0','L1R0U1','L1R0U2','L1R0U3',\n",
    "                'L1R1U0','L1R1U1','L1R1U2','L1R1U3',\n",
    "                'L1R2U0','L1R2U1','L1R2U2','L1R2U3',\n",
    "                'L1R3U0','L1R3U1','L1R3U2','L1R3U3',\n",
    "                'L2R0U0','L2R0U1','L2R0U2','L2R0U3',\n",
    "                'L2R1U0','L2R1U1','L2R1U2','L2R1U3',\n",
    "                'L2R2U0','L2R2U1','L2R2U2','L2R2U3',\n",
    "                'L2R3U0','L2R3U1','L2R3U2','L2R3U3',\n",
    "                'L3R0U0','L3R0U1','L3R0U2','L3R0U3',\n",
    "                'L3R1U0','L3R1U1','L3R1U2','L3R1U3',\n",
    "                'L3R2U0','L3R2U1','L3R2U2','L3R2U3',\n",
    "                'L3R3U0','L3R3U1','L3R3U2','L3R3U3'])\n",
    "Q_table2_gait_state=np.array(['L60D0/30/H0','L180D0/30/H0','L250D0/30/H0','L300D0/30/H0',\n",
    "'L60D30/60/H0','L180D30/60/H0','L250D30/60/H0','L300D30/60/H0',\n",
    "'L60D60/90/H0','L180D60/90/H0','L250D60/90/H0','L300D60/90/H0',\n",
    "'L60D90/120/H0','L180D90/120/H0','L250D90/120/H0','L300D90/120/H0',\n",
    "'L60D120/150/H0','L180D120/150/H0','L250D120/150/H0','L300D120/150/H0',\n",
    "'L60D150/180/H0','L180D150/180/H0','L250D150/180/H0','L300D150/180/H0',\n",
    "'L60D180/210/H0','L180D180/210/H0','L250D180/210/H0','L300D180/210/H0',\n",
    "'L60D210/240/H0','L180D210/240/H0','L250D210/240/H0','L300D210/240/H0',\n",
    "'L60D240/270/H0','L180D240/270/H0','L250D240/270/H0','L300D240/270/H0',\n",
    "'L60D270/300/H0','L180D270/300/H0','L250D270/300/H0','L300D270/300/H0',\n",
    "'L60D300/330/H0','L180D300/330/H0','L250D300/330/H0','L300D300/330/H0',\n",
    "'L60D330/360/H0','L180D330/360/H0','L250D330/360/H0','L300D330/360/H0',\n",
    "'L60D0/30/H1','L180D0/30/H1','L250D0/30/H1','L300D0/30/H1',\n",
    "'L60D30/60/H1','L180D30/60/H1','L250D30/60/H1','L300D30/60/H1',\n",
    "'L60D60/90/H1','L180D60/90/H1','L250D60/90/H1','L300D60/90/H1',\n",
    "'L60D90/120/H1','L180D90/120/H1','L250D90/120/H1','L300D90/120/H1',\n",
    "'L60D120/150/H1','L180D120/150/H1','L250D120/150/H1','L300D120/150/H1',\n",
    "'L60D150/180/H1','L180D150/180/H1','L250D150/180/H1','L300D150/180/H1',\n",
    "'L60D180/210/H1','L180D180/210/H1','L250D180/210/H1','L300D180/210/H1',\n",
    "'L60D210/240/H1','L180D210/240/H1','L250D210/240/H1','L300D210/240/H1',\n",
    "'L60D240/270/H1','L180D240/270/H1','L250D240/270/H1','L300D240/270/H1',\n",
    "'L60D270/300/H1','L180D270/300/H1','L250D270/300/H1','L300D270/300/H1',\n",
    "'L60D300/330/H1','L180D300/330/H1','L250D300/330/H1','L300D300/330/H1',\n",
    "'L60D330/360/H1','L180D330/360/H1','L250D330/360/H1','L300D330/360/H1',\n",
    "'L60D0/30/H2','L180D0/30/H2','L250D0/30/H2','L300D0/30/H2',\n",
    "'L60D30/60/H2','L180D30/60/H2','L250D30/60/H2','L300D30/60/H2',\n",
    "'L60D60/90/H2','L180D60/90/H2','L250D60/90/H2','L300D60/90/H2',\n",
    "'L60D90/120/H2','L180D90/120/H2','L250D90/120/H2','L300D90/120/H2',\n",
    "'L60D120/150/H2','L180D120/150/H2','L250D120/150/H2','L300D120/150/H2',\n",
    "'L60D150/180/H2','L180D150/180/H2','L250D150/180/H2','L300D150/180/H2',\n",
    "'L60D180/210/H2','L180D180/210/H2','L250D180/210/H2','L300D180/210/H2',\n",
    "'L60D210/240/H2','L180D210/240/H2','L250D210/240/H2','L300D210/240/H2',\n",
    "'L60D240/270/H2','L180D240/270/H2','L250D240/270/H2','L300D240/270/H2',\n",
    "'L60D270/300/H2','L180D270/300/H2','L250D270/300/H2','L300D270/300/H2',\n",
    "'L60D300/330/H2','L180D300/330/H2','L250D300/330/H2','L300D300/330/H2',\n",
    "'L60D330/360/H2','L180D330/360/H2','L250D330/360/H2','L300D330/360/H2'])\n",
    "Q_table2_gait_action=['FrontH0','FrontH1','FrontH2','Left_60D','Right_60D']\n",
    "Q_table1_gait_action=['Ahead','Back','Left','Right']"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 45,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "********************** first Q table **********************\n"
     ]
    }
   ],
   "source": [
    "Q_table1_gait=np.loadtxt(r\"E:\\Graduate\\python\\Q_Table_notgait_finish\\Q_Table1_notgait\\Q_Table1_notgait_1.txt\")\n",
    "Q_table1_gait_show=pd.DataFrame(Q_table1_gait,columns=Q_table1_gait_action,index=Q_table1_gait_state)\n",
    "Q_table1_gait_show['Final_action_index']=np.argmax(Q_table1_gait,axis=1)\n",
    "print('********************** first Q table **********************')\n",
    "# print(Q_table1_gait_show)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 46,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "********************** second Q table **********************\n"
     ]
    }
   ],
   "source": [
    "Q_table2_gait=pd.DataFrame([])\n",
    "for i in range(17):\n",
    "    Qtable2_names=((r'E:\\Graduate\\python\\Q_Tablegait6map_finish\\Q_Table2_gait\\Table2gait6map')+str(i)+'.txt')\n",
    "    Load_table=np.loadtxt(Qtable2_names)\n",
    "    Q_table2_gait[('Max')+str(i)]=Load_table\n",
    "f=lambda x:np.argmax(np.bincount(x))\n",
    "Q_table2_gait=pd.DataFrame(Q_table2_gait, dtype=np.int8)\n",
    "Q_table2_gait['Final_action']=Q_table2_gait.apply(f,axis=1)\n",
    "Q_table2_gait.index=Q_table2_gait_state\n",
    "print('********************** second Q table **********************')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 47,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Draw_map_test(Destination_x,Destination_y):\n",
    "    im=Image.new('RGB',size=(2000,2000),color=(0,0,0))\n",
    "    draw=ImageDraw.Draw(im,mode='RGB')\n",
    "    \n",
    "    draw.rectangle((40,40,1960,1960),(255,255,255),(255,255,255))\n",
    "    draw.rectangle((900,1300,1500,1800),(128,138,135),(128,138,135))\n",
    "    draw.rectangle((500,250,800,500),(202,235,216),(202,235,216))\n",
    "    draw.rectangle((1100,300,1800,700),(128,138,135),(128,138,135))\n",
    "    draw.rectangle((100,1000,500,1700),(202,235,216),(202,235,216))\n",
    "    draw.rectangle((750,1000,1300,1300),(202,235,216),(202,235,216))\n",
    "    \n",
    "    \n",
    "    draw.rectangle((1500,1000,1650,1150),(0,0,0), (0,0,0)) \n",
    "    draw.ellipse((400,700,550,850),(0,0,0), (0,0,0)) \n",
    "    draw.rectangle((200,300,350,450),(0,0,0), (0,0,0)) \n",
    "    draw.ellipse((1500,500,1550,650),(0,0,0), (0,0,0)) \n",
    "    draw.ellipse((1200,1400,1350,1550),(0,0,0), (0,0,0)) \n",
    "    draw.rectangle((700,1200,850,1350),(0,0,0), (0,0,0)) \n",
    "    draw.ellipse((300,1600,450,1750),(0,0,0), (0,0,0)) \n",
    "    draw.rectangle((100,1100,250,1250),(0,0,0), (0,0,0)) \n",
    "    draw.ellipse((1100,250,1250,400),(0,0,0), (0,0,0)) \n",
    "    draw.polygon((900, 1070,1120, 1000,1150, 1100, 1100,1090,1050, 1200), (0,0,0), (0,0,0))\n",
    "    draw.pieslice((750, 1700, 900, 1850), 0,180,(0,0,0),(0,0,0))\n",
    "    draw.ellipse((900,550,1050,700),(0,0,0), (0,0,0))\n",
    "    draw.ellipse((650,100,750,200),(0,0,0), (0,0,0)) \n",
    "    draw.ellipse((Destination_x-20,Destination_y-20,Destination_x+20,Destination_y+20),(218,112,214), (218,112,214))\n",
    "    return im\n",
    "im1=Draw_map_test(1000,500)\n",
    "plt.imshow(im1)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 48,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Set_destination(x,y):\n",
    "    return x,y\n",
    "\n",
    "def Start_point(x,y,angle):\n",
    "    return x,y,angle"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 49,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Scan_1(Current_x,Current_y,Angle,im):\n",
    "    Dis_level=0\n",
    "    Obs_dis=Robot_R\n",
    "    Obs_dis_x=Current_x+Obs_dis*np.cos(Angle/180*np.pi)\n",
    "    Obs_dis_y=Current_y+Obs_dis*np.sin(Angle/180*np.pi)\n",
    "    while(im.getpixel((Obs_dis_x,Obs_dis_y))!=(0,0,0) and Obs_dis<250):#getpixiel obtains the degree of Gray Scale\n",
    "        Obs_dis+=5  #Search interval, can be changed\n",
    "        Obs_dis_x=Current_x+Obs_dis*np.cos(Angle/180*np.pi)\n",
    "        Obs_dis_y=Current_y+Obs_dis*np.sin(Angle/180*np.pi)\n",
    "    if 0<=Obs_dis<90:\n",
    "        dis_level=0 #Nearear\n",
    "    elif 90<=Obs_dis<170:\n",
    "        dis_level=1 #Near\n",
    "    elif 170<=Obs_dis<250:\n",
    "        dis_level=2 #Medium\n",
    "    else:\n",
    "        dis_level=3 #Far        \n",
    "    return dis_level,Obs_dis\n",
    "\n",
    "def Direction_min_level_1(Degree,Current_x,Current_y,Rotbot_angle,im):\n",
    "    Level=[]\n",
    "    Dis=[]\n",
    "    Degree=Degree+Rotbot_angle\n",
    "    for i in Degree:\n",
    "        Level.append((Scan_1(Current_x,Current_y,i,im))[0])\n",
    "        Dis.append((Scan_1(Current_x,Current_y,i,im))[1])\n",
    "    return min(Level),min(Dis)\n",
    "\n",
    "def Output_state_index_1(Left_min,Right_min,Up_min):\n",
    "    LRU=[]\n",
    "    LRU.append(Left_min)\n",
    "    LRU.append(Right_min)\n",
    "    LRU.append(Up_min)\n",
    "    return LRU[0]*16+LRU[1]*4+LRU[2]\n",
    "\n",
    "def Is_Crash_1(Current_x,Current_y):\n",
    "    Crash=False\n",
    "    Degree=[-150,-120,-90,-60,-30,0,30,60,90,120,150,180]\n",
    "    Detect_distance=np.arange(0,50,5)\n",
    "    for i in Detect_distance:\n",
    "        for j in Degree:\n",
    "            x=Current_x+i*np.cos(j/180*np.pi)\n",
    "            y=Current_y+i*np.sin(j/180*np.pi)\n",
    "            if (im.getpixel((x,y)))==0:\n",
    "                Crash=True\n",
    "                break\n",
    "        if Crash==True:\n",
    "                break\n",
    "    return Crash"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 50,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Robot_destination_2(Destination_x,Destination_y,Robot_x,Robot_y,Robot_a):\n",
    "    Delta_x=Destination_x-Robot_x\n",
    "    Delta_y=Destination_y-Robot_y\n",
    "    Distance=np.sqrt(Delta_x*Delta_x+Delta_y*Delta_y)\n",
    "    Direct_angle=(np.arctan(Delta_y/Delta_x))*180/np.pi\n",
    "    if Delta_x<0:\n",
    "        Direct_angle=Direct_angle+180\n",
    "    True_angle=(Robot_a-Direct_angle)%360\n",
    "    if True_angle>180:\n",
    "        return 360-True_angle,'right',Distance\n",
    "    else:\n",
    "        return True_angle,'left',Distance\n",
    "    \n",
    "def Is_arrive_2(Current_x,Current_y,im):\n",
    "    Arrive=False\n",
    "    Crash=False\n",
    "    Degree=[-150,-120,-90,-60,-30,0,30,60,90,120,150,180]\n",
    "    Dis=np.arange(0,30,5)\n",
    "    for i in Dis:\n",
    "        for j in Degree:\n",
    "            x=Current_x+i*np.cos(j/180*np.pi)\n",
    "            y=Current_y+i*np.sin(j/180*np.pi)\n",
    "            if (im.getpixel((x,y)))==(0,0,0):\n",
    "                Crash=True\n",
    "                break\n",
    "            if (im.getpixel((x,y)))==(255,0,0):\n",
    "                Arrive=True\n",
    "                break\n",
    "        if Arrive==True or Crash==True:\n",
    "                break\n",
    "    return Arrive\n",
    "\n",
    "def Height_level_front_2(Current_x,Current_y,Current_a,im):\n",
    "    Degree=[-30,0,30]\n",
    "    Distance=np.arange(0,30,5)\n",
    "    Max_height=0\n",
    "    for i in Degree:\n",
    "        for j in Distance:\n",
    "            x=Current_x+i*np.cos((j+Current_a)/180*np.pi)\n",
    "            y=Current_y+i*np.sin((j+Current_a)/180*np.pi)\n",
    "            Height_level=0\n",
    "            if im.getpixel((x,y))==(255,255,255):\n",
    "                Height_level=0\n",
    "            elif im.getpixel((x,y))==(202,235,216):\n",
    "                Height_level=1\n",
    "            elif im.getpixel((x,y))==(128,138,135):\n",
    "                Height_level=2\n",
    "            Max_height=max(Height_level,Max_height)\n",
    "            if Max_height==2:\n",
    "                break\n",
    "        if Max_height==2:\n",
    "            break\n",
    "    return Height_level\n",
    "\n",
    "def Height_level_back_2(Current_x,Current_y,Current_a,im):\n",
    "    Degree=[150,180,-150]\n",
    "    Distance=np.arange(0,30,5)\n",
    "    Max_height=0\n",
    "    for i in Degree:\n",
    "        for j in Distance:\n",
    "            x=Current_x+i*np.cos((j+Current_a)/180*np.pi)\n",
    "            y=Current_y+i*np.sin((j+Current_a)/180*np.pi)\n",
    "            Height_level=0\n",
    "            if im.getpixel((x,y))==(255,255,255):\n",
    "                Height_level=0\n",
    "            elif im.getpixel((x,y))==(202,235,216):\n",
    "                Height_level=1\n",
    "            elif im.getpixel((x,y))==(128,138,135):\n",
    "                Height_level=2\n",
    "            Max_height=max(Height_level,Max_height)\n",
    "            if Max_height==2:\n",
    "                break\n",
    "        if Max_height==2:\n",
    "            break\n",
    "    return Height_level\n",
    "\n",
    "def Output_state_index_2(True_angle,Left_right,Distance,Height):\n",
    "    if 0<=Distance<60:\n",
    "        Dis_level=0\n",
    "    elif 60<=Distance<180:\n",
    "        Dis_level=1\n",
    "    elif 180<=Distance<250:\n",
    "        Dis_level=2\n",
    "    else:\n",
    "        Dis_level=3\n",
    "    Angle_level=True_angle//30\n",
    "    if Left_right=='right':\n",
    "        Le_ri=1\n",
    "        State_number=int(Height*48+Dis_level+24*Le_ri+(5-Angle_level)*4)\n",
    "    elif Left_right=='left':\n",
    "        Le_ri=0\n",
    "        Angle_level=True_angle//30\n",
    "        State_number=int(Height*48+Dis_level+24*Le_ri+Angle_level*4)\n",
    "    return State_number,Dis_level,Angle_level\n",
    "\n",
    "def Height_level_2(Current_x,Current_y,Current_a,im):\n",
    "    Degree=[-30,0,30]\n",
    "    Distance=np.arange(0,30,5)\n",
    "    Max_height=0\n",
    "    for i in Degree:\n",
    "        for j in Distance:\n",
    "            x=Current_x+i*np.cos((j+Current_a)/180*np.pi)\n",
    "            y=Current_y+i*np.sin((j+Current_a)/180*np.pi)\n",
    "            Height_level=0\n",
    "            if im.getpixel((x,y))==(255,255,255):\n",
    "                Height_level=0\n",
    "            elif im.getpixel((x,y))==(202,235,216):\n",
    "                Height_level=1\n",
    "            elif im.getpixel((x,y))==(128,138,135):\n",
    "                Height_level=2\n",
    "            Max_height=max(Height_level,Max_height)\n",
    "            if Max_height==2:\n",
    "                break\n",
    "        if Max_height==2:\n",
    "            break\n",
    "    return Height_level"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 51,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Which_table_action(Avoid_action,Arrive_action,Current_x,Current_y,Current_angle,Destination_x,Destination_y,im):\n",
    "    if Avoid_action==0:\n",
    "        Height_level=Height_level_front_2(Current_x,Current_y,Current_angle,im)\n",
    "        if Height_level==0:\n",
    "            Next_avoid_x=Current_x+Velocity_tripod*np.cos(Current_angle/180*np.pi)\n",
    "            Next_avoid_y=Current_y+Velocity_tripod*np.sin(Current_angle/180*np.pi)\n",
    "            Next_avoid_angle=Current_angle\n",
    "            Next_avoid_action=0\n",
    "        elif Height_level==1:\n",
    "            Next_avoid_x=Current_x+Velocity_quadruped*np.cos(Current_angle/180*np.pi)\n",
    "            Next_avoid_y=Current_y+Velocity_quadruped*np.sin(Current_angle/180*np.pi)\n",
    "            Next_avoid_angle=Current_angle\n",
    "            Next_avoid_action=1\n",
    "        elif Height_level==2:\n",
    "            Next_avoid_x=Current_x+Velocity_onebyone*np.cos(Current_angle/180*np.pi)\n",
    "            Next_avoid_y=Current_y+Velocity_onebyone*np.sin(Current_angle/180*np.pi)\n",
    "            Next_avoid_angle=Current_angle\n",
    "            Next_avoid_action=2\n",
    "    elif Avoid_action==1:\n",
    "        Height_level=Height_level_back_2(Current_x,Current_y,Current_angle,im)\n",
    "        if Height_level==0:\n",
    "            Next_avoid_x=Current_x-Velocity_tripod*np.cos(Current_angle/180*np.pi)\n",
    "            Next_avoid_y=Current_y-Velocity_tripod*np.sin(Current_angle/180*np.pi)\n",
    "            Next_avoid_angle=Current_angle\n",
    "            Next_avoid_action=0\n",
    "        elif Height_level==1:\n",
    "            Next_avoid_x=Current_x-Velocity_quadruped*np.cos(Current_angle/180*np.pi)\n",
    "            Next_avoid_y=Current_y-Velocity_quadruped*np.sin(Current_angle/180*np.pi)\n",
    "            Next_avoid_angle=Current_angle\n",
    "            Next_avoid_action=1\n",
    "        elif Height_level==2:\n",
    "            Next_avoid_x=Current_x-Velocity_onebyone*np.cos(Current_angle/180*np.pi)\n",
    "            Next_avoid_y=Current_y-Velocity_onebyone*np.sin(Current_angle/180*np.pi)\n",
    "            Next_avoid_angle=Current_angle\n",
    "            Next_avoid_action=2\n",
    "    elif Avoid_action==2:\n",
    "        Next_avoid_x=Current_x\n",
    "        Next_avoid_y=Current_y\n",
    "        Next_avoid_angle=Current_angle-45\n",
    "        Next_avoid_action=3\n",
    "    elif Avoid_action==3:\n",
    "        Next_avoid_x=Current_x\n",
    "        Next_avoid_y=Current_y\n",
    "        Next_avoid_angle=Current_angle+45\n",
    "        Next_avoid_action=4\n",
    "    \n",
    "    if Arrive_action==0:\n",
    "        Next_arrive_x=Current_x+Velocity_tripod*np.cos(Current_angle/180*np.pi)\n",
    "        Next_arrive_y=Current_y+Velocity_tripod*np.sin(Current_angle/180*np.pi)\n",
    "        Next_arrive_angle=Current_angle\n",
    "        Next_arrive_action=0\n",
    "    elif Arrive_action==1:\n",
    "        Next_arrive_x=Current_x+Velocity_quadruped*np.cos(Current_angle/180*np.pi)\n",
    "        Next_arrive_y=Current_y+Velocity_quadruped*np.sin(Current_angle/180*np.pi)\n",
    "        Next_arrive_angle=Current_angle\n",
    "        Next_arrive_action=1\n",
    "    elif Arrive_action==2:\n",
    "        Next_arrive_x=Current_x+Velocity_onebyone*np.cos(Current_angle/180*np.pi)\n",
    "        Next_arrive_y=Current_y+Velocity_onebyone*np.sin(Current_angle/180*np.pi)\n",
    "        Next_arrive_angle=Current_angle\n",
    "        Next_arrive_action=2\n",
    "    elif Arrive_action==3:\n",
    "        Next_arrive_x=Current_x\n",
    "        Next_arrive_y=Current_y\n",
    "        Next_arrive_angle=Current_angle-60\n",
    "        Next_arrive_action=3\n",
    "    elif Arrive_action==4:\n",
    "        Next_arrive_x=Current_x\n",
    "        Next_arrive_y=Current_y\n",
    "        Next_arrive_angle=Current_angle+60\n",
    "        Next_arrive_action=4\n",
    "        \n",
    "    Next_left_level_1,Next_left_obstacle_distance_1=Direction_min_level_1(Left_D,Next_avoid_x,Next_avoid_y,Next_avoid_angle,im)\n",
    "    Next_right_level_1,Next_right_obstacle_distance_1=Direction_min_level_1(Right_D,Next_avoid_x,Next_avoid_y,Next_avoid_angle,im)\n",
    "    Next_up_level_1,Next_up_obstacle_disance_1=Direction_min_level_1(Up_D,Next_avoid_x,Next_avoid_y,Next_avoid_angle,im)\n",
    "    Next_min_level_1=min(Next_left_level_1, Next_right_level_1,Next_up_level_1)\n",
    "    \n",
    "    if Next_min_level_1==0:\n",
    "        Next_x=Next_avoid_x\n",
    "        Next_y=Next_avoid_y\n",
    "        Next_angle=Next_avoid_angle\n",
    "#         Next_action='AVOID'+str(Avoid_action)\n",
    "        Next_action=Next_avoid_action\n",
    "        \n",
    "    else:\n",
    "        Next_x=Next_arrive_x\n",
    "        Next_y=Next_arrive_y\n",
    "        Next_angle=Next_arrive_angle\n",
    "        Next_action=Next_arrive_action\n",
    "#         Next_action='ARRIVE'+str(Arrive_action)\n",
    "        \n",
    "    \n",
    "    return Next_x,Next_y,Next_angle,Next_action\n",
    "        \n",
    "        "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 52,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Movement_plot(Vec_x_tripod,Vec_y_tripod,Vec_x_quadruped,Vec_y_quadruped,Vec_x_onebyone,Vec_y_onebyone,\n",
    "                  Vec_x_turn,Vec_y_turn,Destination_x,Destination_y):\n",
    "    \n",
    "    im=Image.new('RGB',size=(2000,2000),color=(0,0,0))\n",
    "    draw=ImageDraw.Draw(im,mode='RGB')\n",
    "    \n",
    "    draw.rectangle((40,40,1960,1960),(255,255,255),(255,255,255))\n",
    "    draw.rectangle((900,1300,1500,1800),(128,138,135),(128,138,135))\n",
    "    draw.rectangle((500,250,800,500),(106,90,205),(106,90,205))\n",
    "    draw.rectangle((1100,300,1800,700),(128,138,135),(128,138,135))\n",
    "    draw.rectangle((100,1000,500,1700),(106,90,205),(106,90,205))\n",
    "    draw.rectangle((750,1000,1300,1300),(106,90,205),(106,90,205))\n",
    "    \n",
    "    draw.rectangle((1500,1000,1650,1150),(0,0,0), (0,0,0)) \n",
    "    draw.ellipse((400,700,550,850),(0,0,0), (0,0,0)) \n",
    "    draw.rectangle((200,300,350,450),(0,0,0), (0,0,0)) \n",
    "    draw.ellipse((1500,500,1550,650),(0,0,0), (0,0,0)) \n",
    "    draw.ellipse((1200,1400,1350,1550),(0,0,0), (0,0,0)) \n",
    "    draw.rectangle((700,1200,850,1350),(0,0,0), (0,0,0)) \n",
    "    draw.ellipse((300,1600,450,1750),(0,0,0), (0,0,0)) \n",
    "    draw.rectangle((100,1100,250,1250),(0,0,0), (0,0,0)) \n",
    "    draw.ellipse((1100,250,1250,400),(0,0,0), (0,0,0)) \n",
    "    draw.polygon((900, 1070,1120, 1000,1150, 1100, 1100,1090,1050, 1200), (0,0,0), (0,0,0))\n",
    "    draw.pieslice((750, 1700, 900, 1850), 0,180,(0,0,0),(0,0,0))\n",
    "    draw.ellipse((900,550,1050,700),(0,0,0), (0,0,0))\n",
    "    draw.ellipse((650,100,750,200),(0,0,0), (0,0,0)) \n",
    "    draw.rectangle((1700,130,1800,230),(0,0,0),(0,0,0))\n",
    "    draw.ellipse((Destination_x-20,Destination_y-20,Destination_x+20,Destination_y+20),(218,112,214), (218,112,214))\n",
    "    \n",
    "    d=1\n",
    "    for d in range(50):\n",
    "        for i,j,k in zip(Vec_x_tripod[0:],Vec_y_tripod[0:],Vec_time_tripod[0:]):\n",
    "            if 10*(d-1)<k<=10*d:\n",
    "                draw.ellipse((i-40,j-40, i+40,j+40),(0,255,0),(0,255,0))\n",
    "        for i,j,k in zip(Vec_x_quadruped[0:],Vec_y_quadruped[0:],Vec_time_quadruped[0:]):\n",
    "            if 10*(d-1)<k<10*d:\n",
    "                draw.ellipse((i-40,j-40, i+40,j+40),(255,255,0),(255,255,0))\n",
    "        for i,j,k in zip(Vec_x_onebyone[0:],Vec_y_onebyone[0:],Vec_time_onebyone[0:]):\n",
    "            if 10*(d-1)<k<10*d:\n",
    "                draw.ellipse((i-40,j-40, i+40,j+40),(255,215,0),(255,215,0))\n",
    "        for i,j,k in zip(Vec_x_turn[0:],Vec_y_turn[0:],Vec_time_turn[0:]):\n",
    "            if 10*(d-1)<k<10*d:\n",
    "                draw.ellipse((i-40,j-40, i+40,j+40),(255,105,180),(255,105,180))\n",
    "        plt.imshow(im)\n",
    "        plt.savefig((r\"E:\\Graduate\\python\\Q_Tablegait6map_finish\\Pic1\\picture\")+str(d)+\".png\")\n",
    "        d+=1\n",
    "    return im"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 53,
   "metadata": {},
   "outputs": [],
   "source": [
    "global Vec_x_tripod\n",
    "global Vec_y_tripod\n",
    "global Vec_x_quadruped\n",
    "global Vec_y_quadruped\n",
    "global Vec_x_onebyone\n",
    "global Vec_y_onebyone\n",
    "global Vec_x_turn\n",
    "global Vec_y_turn\n",
    "global Vec_time_tripod\n",
    "global Vec_time_quadruped\n",
    "global Vec_time_onebyone\n",
    "global Vec_time_turn"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 54,
   "metadata": {
    "scrolled": true
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "**********************************************************\n",
      "Current_x=500.000000    Current_y=1500.000000   Current_angle=30.000000\n",
      "3\n",
      "Next_x=500.000000    Next_y=1500.000000    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=500.000000    Current_y=1500.000000   Current_angle=-30.000000\n",
      "0\n",
      "Next_x=510.011254    Next_y=1494.220000    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=510.011254    Current_y=1494.220000   Current_angle=-30.000000\n",
      "0\n",
      "Next_x=520.022507    Next_y=1488.440000    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=520.022507    Current_y=1488.440000   Current_angle=-30.000000\n",
      "0\n",
      "Next_x=530.033761    Next_y=1482.660000    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=530.033761    Current_y=1482.660000   Current_angle=-30.000000\n",
      "0\n",
      "Next_x=540.045015    Next_y=1476.880000    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=540.045015    Current_y=1476.880000   Current_angle=-30.000000\n",
      "0\n",
      "Next_x=550.056268    Next_y=1471.100000    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=550.056268    Current_y=1471.100000   Current_angle=-30.000000\n",
      "0\n",
      "Next_x=560.067522    Next_y=1465.320000    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=560.067522    Current_y=1465.320000   Current_angle=-30.000000\n",
      "0\n",
      "Next_x=570.078776    Next_y=1459.540000    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=570.078776    Current_y=1459.540000   Current_angle=-30.000000\n",
      "0\n",
      "Next_x=580.090029    Next_y=1453.760000    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=580.090029    Current_y=1453.760000   Current_angle=-30.000000\n",
      "0\n",
      "Next_x=590.101283    Next_y=1447.980000    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=590.101283    Current_y=1447.980000   Current_angle=-30.000000\n",
      "0\n",
      "Next_x=600.112537    Next_y=1442.200000    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=600.112537    Current_y=1442.200000   Current_angle=-30.000000\n",
      "0\n",
      "Next_x=610.123790    Next_y=1436.420000    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=610.123790    Current_y=1436.420000   Current_angle=-30.000000\n",
      "0\n",
      "Next_x=620.135044    Next_y=1430.640000    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=620.135044    Current_y=1430.640000   Current_angle=-30.000000\n",
      "0\n",
      "Next_x=630.146298    Next_y=1424.860000    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=630.146298    Current_y=1424.860000   Current_angle=-30.000000\n",
      "0\n",
      "Next_x=640.157551    Next_y=1419.080000    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=640.157551    Current_y=1419.080000   Current_angle=-30.000000\n",
      "0\n",
      "Next_x=650.168805    Next_y=1413.300000    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=650.168805    Current_y=1413.300000   Current_angle=-30.000000\n",
      "0\n",
      "Next_x=660.180059    Next_y=1407.520000    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=660.180059    Current_y=1407.520000   Current_angle=-30.000000\n",
      "4\n",
      "Next_x=660.180059    Next_y=1407.520000    Next_angle=15.000000\n",
      "**********************************************************\n",
      "Current_x=660.180059    Current_y=1407.520000   Current_angle=15.000000\n",
      "0\n",
      "Next_x=671.346161    Next_y=1410.511948    Next_angle=15.000000\n",
      "**********************************************************\n",
      "Current_x=671.346161    Current_y=1410.511948   Current_angle=15.000000\n",
      "0\n",
      "Next_x=682.512264    Next_y=1413.503896    Next_angle=15.000000\n",
      "**********************************************************\n",
      "Current_x=682.512264    Current_y=1413.503896   Current_angle=15.000000\n",
      "0\n",
      "Next_x=693.678366    Next_y=1416.495844    Next_angle=15.000000\n",
      "**********************************************************\n",
      "Current_x=693.678366    Current_y=1416.495844   Current_angle=15.000000\n",
      "0\n",
      "Next_x=704.844469    Next_y=1419.487793    Next_angle=15.000000\n",
      "**********************************************************\n",
      "Current_x=704.844469    Current_y=1419.487793   Current_angle=15.000000\n",
      "0\n",
      "Next_x=716.010571    Next_y=1422.479741    Next_angle=15.000000\n",
      "**********************************************************\n",
      "Current_x=716.010571    Current_y=1422.479741   Current_angle=15.000000\n",
      "0\n",
      "Next_x=727.176674    Next_y=1425.471689    Next_angle=15.000000\n",
      "**********************************************************\n",
      "Current_x=727.176674    Current_y=1425.471689   Current_angle=15.000000\n",
      "0\n",
      "Next_x=738.342777    Next_y=1428.463637    Next_angle=15.000000\n",
      "**********************************************************\n",
      "Current_x=738.342777    Current_y=1428.463637   Current_angle=15.000000\n",
      "0\n",
      "Next_x=749.508879    Next_y=1431.455585    Next_angle=15.000000\n",
      "**********************************************************\n",
      "Current_x=749.508879    Current_y=1431.455585   Current_angle=15.000000\n",
      "0\n",
      "Next_x=760.674982    Next_y=1434.447533    Next_angle=15.000000\n",
      "**********************************************************\n",
      "Current_x=760.674982    Current_y=1434.447533   Current_angle=15.000000\n",
      "3\n",
      "Next_x=760.674982    Next_y=1434.447533    Next_angle=-45.000000\n",
      "**********************************************************\n",
      "Current_x=760.674982    Current_y=1434.447533   Current_angle=-45.000000\n",
      "4\n",
      "Next_x=760.674982    Next_y=1434.447533    Next_angle=0.000000\n",
      "**********************************************************\n",
      "Current_x=760.674982    Current_y=1434.447533   Current_angle=0.000000\n",
      "0\n",
      "Next_x=772.234982    Next_y=1434.447533    Next_angle=0.000000\n",
      "**********************************************************\n",
      "Current_x=772.234982    Current_y=1434.447533   Current_angle=0.000000\n",
      "0\n",
      "Next_x=783.794982    Next_y=1434.447533    Next_angle=0.000000\n",
      "**********************************************************\n",
      "Current_x=783.794982    Current_y=1434.447533   Current_angle=0.000000\n",
      "0\n",
      "Next_x=795.354982    Next_y=1434.447533    Next_angle=0.000000\n",
      "**********************************************************\n",
      "Current_x=795.354982    Current_y=1434.447533   Current_angle=0.000000\n",
      "0\n",
      "Next_x=806.914982    Next_y=1434.447533    Next_angle=0.000000\n",
      "**********************************************************\n",
      "Current_x=806.914982    Current_y=1434.447533   Current_angle=0.000000\n",
      "0\n",
      "Next_x=818.474982    Next_y=1434.447533    Next_angle=0.000000\n",
      "**********************************************************\n",
      "Current_x=818.474982    Current_y=1434.447533   Current_angle=0.000000\n",
      "0\n",
      "Next_x=830.034982    Next_y=1434.447533    Next_angle=0.000000\n",
      "**********************************************************\n",
      "Current_x=830.034982    Current_y=1434.447533   Current_angle=0.000000\n",
      "0\n",
      "Next_x=841.594982    Next_y=1434.447533    Next_angle=0.000000\n",
      "**********************************************************\n",
      "Current_x=841.594982    Current_y=1434.447533   Current_angle=0.000000\n",
      "0\n",
      "Next_x=853.154982    Next_y=1434.447533    Next_angle=0.000000\n",
      "**********************************************************\n",
      "Current_x=853.154982    Current_y=1434.447533   Current_angle=0.000000\n",
      "0\n",
      "Next_x=864.714982    Next_y=1434.447533    Next_angle=0.000000\n",
      "**********************************************************\n",
      "Current_x=864.714982    Current_y=1434.447533   Current_angle=0.000000\n",
      "3\n",
      "Next_x=864.714982    Next_y=1434.447533    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=864.714982    Current_y=1434.447533   Current_angle=-60.000000\n",
      "0\n",
      "Next_x=870.494982    Next_y=1424.436280    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=870.494982    Current_y=1424.436280   Current_angle=-60.000000\n",
      "0\n",
      "Next_x=876.274982    Next_y=1414.425026    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=876.274982    Current_y=1414.425026   Current_angle=-60.000000\n",
      "2\n",
      "Next_x=878.194982    Next_y=1411.099489    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=878.194982    Current_y=1411.099489   Current_angle=-60.000000\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "2\n",
      "Next_x=880.114982    Next_y=1407.773951    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=880.114982    Current_y=1407.773951   Current_angle=-60.000000\n",
      "2\n",
      "Next_x=882.034982    Next_y=1404.448413    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=882.034982    Current_y=1404.448413   Current_angle=-60.000000\n",
      "2\n",
      "Next_x=883.954982    Next_y=1401.122876    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=883.954982    Current_y=1401.122876   Current_angle=-60.000000\n",
      "2\n",
      "Next_x=885.874982    Next_y=1397.797338    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=885.874982    Current_y=1397.797338   Current_angle=-60.000000\n",
      "2\n",
      "Next_x=887.794982    Next_y=1394.471801    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=887.794982    Current_y=1394.471801   Current_angle=-60.000000\n",
      "2\n",
      "Next_x=889.714982    Next_y=1391.146263    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=889.714982    Current_y=1391.146263   Current_angle=-60.000000\n",
      "2\n",
      "Next_x=891.634982    Next_y=1387.820726    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=891.634982    Current_y=1387.820726   Current_angle=-60.000000\n",
      "2\n",
      "Next_x=893.554982    Next_y=1384.495188    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=893.554982    Current_y=1384.495188   Current_angle=-60.000000\n",
      "2\n",
      "Next_x=895.474982    Next_y=1381.169651    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=895.474982    Current_y=1381.169651   Current_angle=-60.000000\n",
      "2\n",
      "Next_x=897.394982    Next_y=1377.844113    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=897.394982    Current_y=1377.844113   Current_angle=-60.000000\n",
      "2\n",
      "Next_x=899.314982    Next_y=1374.518576    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=899.314982    Current_y=1374.518576   Current_angle=-60.000000\n",
      "2\n",
      "Next_x=901.234982    Next_y=1371.193038    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=901.234982    Current_y=1371.193038   Current_angle=-60.000000\n",
      "2\n",
      "Next_x=903.154982    Next_y=1367.867500    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=903.154982    Current_y=1367.867500   Current_angle=-60.000000\n",
      "2\n",
      "Next_x=905.074982    Next_y=1364.541963    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=905.074982    Current_y=1364.541963   Current_angle=-60.000000\n",
      "2\n",
      "Next_x=906.994982    Next_y=1361.216425    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=906.994982    Current_y=1361.216425   Current_angle=-60.000000\n",
      "2\n",
      "Next_x=908.914982    Next_y=1357.890888    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=908.914982    Current_y=1357.890888   Current_angle=-60.000000\n",
      "2\n",
      "Next_x=910.834982    Next_y=1354.565350    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=910.834982    Current_y=1354.565350   Current_angle=-60.000000\n",
      "2\n",
      "Next_x=912.754982    Next_y=1351.239813    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=912.754982    Current_y=1351.239813   Current_angle=-60.000000\n",
      "2\n",
      "Next_x=914.674982    Next_y=1347.914275    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=914.674982    Current_y=1347.914275   Current_angle=-60.000000\n",
      "2\n",
      "Next_x=916.594982    Next_y=1344.588738    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=916.594982    Current_y=1344.588738   Current_angle=-60.000000\n",
      "2\n",
      "Next_x=918.514982    Next_y=1341.263200    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=918.514982    Current_y=1341.263200   Current_angle=-60.000000\n",
      "2\n",
      "Next_x=920.434982    Next_y=1337.937662    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=920.434982    Current_y=1337.937662   Current_angle=-60.000000\n",
      "2\n",
      "Next_x=922.354982    Next_y=1334.612125    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=922.354982    Current_y=1334.612125   Current_angle=-60.000000\n",
      "2\n",
      "Next_x=924.274982    Next_y=1331.286587    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=924.274982    Current_y=1331.286587   Current_angle=-60.000000\n",
      "2\n",
      "Next_x=926.194982    Next_y=1327.961050    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=926.194982    Current_y=1327.961050   Current_angle=-60.000000\n",
      "2\n",
      "Next_x=928.114982    Next_y=1324.635512    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=928.114982    Current_y=1324.635512   Current_angle=-60.000000\n",
      "2\n",
      "Next_x=930.034982    Next_y=1321.309975    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=930.034982    Current_y=1321.309975   Current_angle=-60.000000\n",
      "2\n",
      "Next_x=931.954982    Next_y=1317.984437    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=931.954982    Current_y=1317.984437   Current_angle=-60.000000\n",
      "2\n",
      "Next_x=933.874982    Next_y=1314.658900    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=933.874982    Current_y=1314.658900   Current_angle=-60.000000\n",
      "2\n",
      "Next_x=935.794982    Next_y=1311.333362    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=935.794982    Current_y=1311.333362   Current_angle=-60.000000\n",
      "2\n",
      "Next_x=937.714982    Next_y=1308.007824    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=937.714982    Current_y=1308.007824   Current_angle=-60.000000\n",
      "2\n",
      "Next_x=939.634982    Next_y=1304.682287    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=939.634982    Current_y=1304.682287   Current_angle=-60.000000\n",
      "2\n",
      "Next_x=941.554982    Next_y=1301.356749    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=941.554982    Current_y=1301.356749   Current_angle=-60.000000\n",
      "2\n",
      "Next_x=943.474982    Next_y=1298.031212    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=943.474982    Current_y=1298.031212   Current_angle=-60.000000\n",
      "2\n",
      "Next_x=945.394982    Next_y=1294.705674    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=945.394982    Current_y=1294.705674   Current_angle=-60.000000\n",
      "2\n",
      "Next_x=947.314982    Next_y=1291.380137    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=947.314982    Current_y=1291.380137   Current_angle=-60.000000\n",
      "2\n",
      "Next_x=949.234982    Next_y=1288.054599    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=949.234982    Current_y=1288.054599   Current_angle=-60.000000\n",
      "2\n",
      "Next_x=951.154982    Next_y=1284.729062    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=951.154982    Current_y=1284.729062   Current_angle=-60.000000\n",
      "2\n",
      "Next_x=953.074982    Next_y=1281.403524    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=953.074982    Current_y=1281.403524   Current_angle=-60.000000\n",
      "2\n",
      "Next_x=954.994982    Next_y=1278.077987    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=954.994982    Current_y=1278.077987   Current_angle=-60.000000\n",
      "2\n",
      "Next_x=956.914982    Next_y=1274.752449    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=956.914982    Current_y=1274.752449   Current_angle=-60.000000\n",
      "1\n",
      "Next_x=959.214982    Next_y=1270.768732    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=959.214982    Current_y=1270.768732   Current_angle=-60.000000\n",
      "1\n",
      "Next_x=961.514982    Next_y=1266.785015    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=961.514982    Current_y=1266.785015   Current_angle=-60.000000\n",
      "1\n",
      "Next_x=963.814982    Next_y=1262.801298    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=963.814982    Current_y=1262.801298   Current_angle=-60.000000\n",
      "1\n",
      "Next_x=966.114982    Next_y=1258.817582    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=966.114982    Current_y=1258.817582   Current_angle=-60.000000\n",
      "1\n",
      "Next_x=968.414982    Next_y=1254.833865    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=968.414982    Current_y=1254.833865   Current_angle=-60.000000\n",
      "1\n",
      "Next_x=970.714982    Next_y=1250.850148    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=970.714982    Current_y=1250.850148   Current_angle=-60.000000\n",
      "1\n",
      "Next_x=973.014982    Next_y=1246.866431    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=973.014982    Current_y=1246.866431   Current_angle=-60.000000\n",
      "1\n",
      "Next_x=975.314982    Next_y=1242.882714    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=975.314982    Current_y=1242.882714   Current_angle=-60.000000\n",
      "4\n",
      "Next_x=975.314982    Next_y=1242.882714    Next_angle=-15.000000\n",
      "**********************************************************\n",
      "Current_x=975.314982    Current_y=1242.882714   Current_angle=-15.000000\n",
      "4\n",
      "Next_x=975.314982    Next_y=1242.882714    Next_angle=30.000000\n",
      "**********************************************************\n",
      "Current_x=975.314982    Current_y=1242.882714   Current_angle=30.000000\n",
      "1\n",
      "Next_x=979.298699    Next_y=1245.182714    Next_angle=30.000000\n",
      "**********************************************************\n",
      "Current_x=979.298699    Current_y=1245.182714   Current_angle=30.000000\n",
      "1\n",
      "Next_x=983.282415    Next_y=1247.482714    Next_angle=30.000000\n",
      "**********************************************************\n",
      "Current_x=983.282415    Current_y=1247.482714   Current_angle=30.000000\n",
      "1\n",
      "Next_x=987.266132    Next_y=1249.782714    Next_angle=30.000000\n",
      "**********************************************************\n",
      "Current_x=987.266132    Current_y=1249.782714   Current_angle=30.000000\n",
      "1\n",
      "Next_x=991.249849    Next_y=1252.082714    Next_angle=30.000000\n",
      "**********************************************************\n",
      "Current_x=991.249849    Current_y=1252.082714   Current_angle=30.000000\n",
      "1\n",
      "Next_x=995.233566    Next_y=1254.382714    Next_angle=30.000000\n",
      "**********************************************************\n",
      "Current_x=995.233566    Current_y=1254.382714   Current_angle=30.000000\n",
      "1\n",
      "Next_x=999.217283    Next_y=1256.682714    Next_angle=30.000000\n",
      "**********************************************************\n",
      "Current_x=999.217283    Current_y=1256.682714   Current_angle=30.000000\n",
      "1\n",
      "Next_x=1003.201000    Next_y=1258.982714    Next_angle=30.000000\n",
      "**********************************************************\n",
      "Current_x=1003.201000    Current_y=1258.982714   Current_angle=30.000000\n",
      "1\n",
      "Next_x=1007.184717    Next_y=1261.282714    Next_angle=30.000000\n",
      "**********************************************************\n",
      "Current_x=1007.184717    Current_y=1261.282714   Current_angle=30.000000\n",
      "1\n",
      "Next_x=1011.168433    Next_y=1263.582714    Next_angle=30.000000\n",
      "**********************************************************\n",
      "Current_x=1011.168433    Current_y=1263.582714   Current_angle=30.000000\n",
      "1\n",
      "Next_x=1015.152150    Next_y=1265.882714    Next_angle=30.000000\n",
      "**********************************************************\n",
      "Current_x=1015.152150    Current_y=1265.882714   Current_angle=30.000000\n",
      "1\n",
      "Next_x=1019.135867    Next_y=1268.182714    Next_angle=30.000000\n",
      "**********************************************************\n",
      "Current_x=1019.135867    Current_y=1268.182714   Current_angle=30.000000\n",
      "1\n",
      "Next_x=1023.119584    Next_y=1270.482714    Next_angle=30.000000\n",
      "**********************************************************\n",
      "Current_x=1023.119584    Current_y=1270.482714   Current_angle=30.000000\n",
      "3\n",
      "Next_x=1023.119584    Next_y=1270.482714    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1023.119584    Current_y=1270.482714   Current_angle=-30.000000\n",
      "3\n",
      "Next_x=1023.119584    Next_y=1270.482714    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1023.119584    Current_y=1270.482714   Current_angle=-90.000000\n",
      "1\n",
      "Next_x=1023.119584    Next_y=1265.882714    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1023.119584    Current_y=1265.882714   Current_angle=-90.000000\n",
      "4\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Next_x=1023.119584    Next_y=1265.882714    Next_angle=-45.000000\n",
      "**********************************************************\n",
      "Current_x=1023.119584    Current_y=1265.882714   Current_angle=-45.000000\n",
      "4\n",
      "Next_x=1023.119584    Next_y=1265.882714    Next_angle=0.000000\n",
      "**********************************************************\n",
      "Current_x=1023.119584    Current_y=1265.882714   Current_angle=0.000000\n",
      "1\n",
      "Next_x=1027.719584    Next_y=1265.882714    Next_angle=0.000000\n",
      "**********************************************************\n",
      "Current_x=1027.719584    Current_y=1265.882714   Current_angle=0.000000\n",
      "1\n",
      "Next_x=1032.319584    Next_y=1265.882714    Next_angle=0.000000\n",
      "**********************************************************\n",
      "Current_x=1032.319584    Current_y=1265.882714   Current_angle=0.000000\n",
      "1\n",
      "Next_x=1036.919584    Next_y=1265.882714    Next_angle=0.000000\n",
      "**********************************************************\n",
      "Current_x=1036.919584    Current_y=1265.882714   Current_angle=0.000000\n",
      "1\n",
      "Next_x=1041.519584    Next_y=1265.882714    Next_angle=0.000000\n",
      "**********************************************************\n",
      "Current_x=1041.519584    Current_y=1265.882714   Current_angle=0.000000\n",
      "1\n",
      "Next_x=1046.119584    Next_y=1265.882714    Next_angle=0.000000\n",
      "**********************************************************\n",
      "Current_x=1046.119584    Current_y=1265.882714   Current_angle=0.000000\n",
      "1\n",
      "Next_x=1050.719584    Next_y=1265.882714    Next_angle=0.000000\n",
      "**********************************************************\n",
      "Current_x=1050.719584    Current_y=1265.882714   Current_angle=0.000000\n",
      "1\n",
      "Next_x=1055.319584    Next_y=1265.882714    Next_angle=0.000000\n",
      "**********************************************************\n",
      "Current_x=1055.319584    Current_y=1265.882714   Current_angle=0.000000\n",
      "1\n",
      "Next_x=1059.919584    Next_y=1265.882714    Next_angle=0.000000\n",
      "**********************************************************\n",
      "Current_x=1059.919584    Current_y=1265.882714   Current_angle=0.000000\n",
      "1\n",
      "Next_x=1064.519584    Next_y=1265.882714    Next_angle=0.000000\n",
      "**********************************************************\n",
      "Current_x=1064.519584    Current_y=1265.882714   Current_angle=0.000000\n",
      "1\n",
      "Next_x=1069.119584    Next_y=1265.882714    Next_angle=0.000000\n",
      "**********************************************************\n",
      "Current_x=1069.119584    Current_y=1265.882714   Current_angle=0.000000\n",
      "1\n",
      "Next_x=1073.719584    Next_y=1265.882714    Next_angle=0.000000\n",
      "**********************************************************\n",
      "Current_x=1073.719584    Current_y=1265.882714   Current_angle=0.000000\n",
      "3\n",
      "Next_x=1073.719584    Next_y=1265.882714    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=1073.719584    Current_y=1265.882714   Current_angle=-60.000000\n",
      "1\n",
      "Next_x=1076.019584    Next_y=1261.898997    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=1076.019584    Current_y=1261.898997   Current_angle=-60.000000\n",
      "1\n",
      "Next_x=1078.319584    Next_y=1257.915280    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=1078.319584    Current_y=1257.915280   Current_angle=-60.000000\n",
      "1\n",
      "Next_x=1080.619584    Next_y=1253.931564    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=1080.619584    Current_y=1253.931564   Current_angle=-60.000000\n",
      "1\n",
      "Next_x=1082.919584    Next_y=1249.947847    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=1082.919584    Current_y=1249.947847   Current_angle=-60.000000\n",
      "1\n",
      "Next_x=1085.219584    Next_y=1245.964130    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=1085.219584    Current_y=1245.964130   Current_angle=-60.000000\n",
      "1\n",
      "Next_x=1087.519584    Next_y=1241.980413    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=1087.519584    Current_y=1241.980413   Current_angle=-60.000000\n",
      "1\n",
      "Next_x=1089.819584    Next_y=1237.996696    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=1089.819584    Current_y=1237.996696   Current_angle=-60.000000\n",
      "1\n",
      "Next_x=1092.119584    Next_y=1234.012979    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=1092.119584    Current_y=1234.012979   Current_angle=-60.000000\n",
      "1\n",
      "Next_x=1094.419584    Next_y=1230.029262    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=1094.419584    Current_y=1230.029262   Current_angle=-60.000000\n",
      "1\n",
      "Next_x=1096.719584    Next_y=1226.045546    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=1096.719584    Current_y=1226.045546   Current_angle=-60.000000\n",
      "1\n",
      "Next_x=1099.019584    Next_y=1222.061829    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=1099.019584    Current_y=1222.061829   Current_angle=-60.000000\n",
      "1\n",
      "Next_x=1101.319584    Next_y=1218.078112    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=1101.319584    Current_y=1218.078112   Current_angle=-60.000000\n",
      "1\n",
      "Next_x=1103.619584    Next_y=1214.094395    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=1103.619584    Current_y=1214.094395   Current_angle=-60.000000\n",
      "1\n",
      "Next_x=1105.919584    Next_y=1210.110678    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=1105.919584    Current_y=1210.110678   Current_angle=-60.000000\n",
      "1\n",
      "Next_x=1108.219584    Next_y=1206.126961    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=1108.219584    Current_y=1206.126961   Current_angle=-60.000000\n",
      "1\n",
      "Next_x=1110.519584    Next_y=1202.143244    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=1110.519584    Current_y=1202.143244   Current_angle=-60.000000\n",
      "1\n",
      "Next_x=1112.819584    Next_y=1198.159528    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=1112.819584    Current_y=1198.159528   Current_angle=-60.000000\n",
      "1\n",
      "Next_x=1115.119584    Next_y=1194.175811    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=1115.119584    Current_y=1194.175811   Current_angle=-60.000000\n",
      "1\n",
      "Next_x=1117.419584    Next_y=1190.192094    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=1117.419584    Current_y=1190.192094   Current_angle=-60.000000\n",
      "1\n",
      "Next_x=1119.719584    Next_y=1186.208377    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=1119.719584    Current_y=1186.208377   Current_angle=-60.000000\n",
      "1\n",
      "Next_x=1122.019584    Next_y=1182.224660    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=1122.019584    Current_y=1182.224660   Current_angle=-60.000000\n",
      "1\n",
      "Next_x=1124.319584    Next_y=1178.240943    Next_angle=-60.000000\n",
      "**********************************************************\n",
      "Current_x=1124.319584    Current_y=1178.240943   Current_angle=-60.000000\n",
      "4\n",
      "Next_x=1124.319584    Next_y=1178.240943    Next_angle=-15.000000\n",
      "**********************************************************\n",
      "Current_x=1124.319584    Current_y=1178.240943   Current_angle=-15.000000\n",
      "1\n",
      "Next_x=1128.762843    Next_y=1177.050376    Next_angle=-15.000000\n",
      "**********************************************************\n",
      "Current_x=1128.762843    Current_y=1177.050376   Current_angle=-15.000000\n",
      "1\n",
      "Next_x=1133.206102    Next_y=1175.859808    Next_angle=-15.000000\n",
      "**********************************************************\n",
      "Current_x=1133.206102    Current_y=1175.859808   Current_angle=-15.000000\n",
      "1\n",
      "Next_x=1137.649360    Next_y=1174.669240    Next_angle=-15.000000\n",
      "**********************************************************\n",
      "Current_x=1137.649360    Current_y=1174.669240   Current_angle=-15.000000\n",
      "1\n",
      "Next_x=1142.092619    Next_y=1173.478673    Next_angle=-15.000000\n",
      "**********************************************************\n",
      "Current_x=1142.092619    Current_y=1173.478673   Current_angle=-15.000000\n",
      "1\n",
      "Next_x=1146.535878    Next_y=1172.288105    Next_angle=-15.000000\n",
      "**********************************************************\n",
      "Current_x=1146.535878    Current_y=1172.288105   Current_angle=-15.000000\n",
      "1\n",
      "Next_x=1150.979137    Next_y=1171.097538    Next_angle=-15.000000\n",
      "**********************************************************\n",
      "Current_x=1150.979137    Current_y=1171.097538   Current_angle=-15.000000\n",
      "1\n",
      "Next_x=1155.422396    Next_y=1169.906970    Next_angle=-15.000000\n",
      "**********************************************************\n",
      "Current_x=1155.422396    Current_y=1169.906970   Current_angle=-15.000000\n",
      "1\n",
      "Next_x=1159.865654    Next_y=1168.716402    Next_angle=-15.000000\n",
      "**********************************************************\n",
      "Current_x=1159.865654    Current_y=1168.716402   Current_angle=-15.000000\n",
      "1\n",
      "Next_x=1164.308913    Next_y=1167.525835    Next_angle=-15.000000\n",
      "**********************************************************\n",
      "Current_x=1164.308913    Current_y=1167.525835   Current_angle=-15.000000\n",
      "1\n",
      "Next_x=1168.752172    Next_y=1166.335267    Next_angle=-15.000000\n",
      "**********************************************************\n",
      "Current_x=1168.752172    Current_y=1166.335267   Current_angle=-15.000000\n",
      "1\n",
      "Next_x=1173.195431    Next_y=1165.144700    Next_angle=-15.000000\n",
      "**********************************************************\n",
      "Current_x=1173.195431    Current_y=1165.144700   Current_angle=-15.000000\n",
      "1\n",
      "Next_x=1177.638690    Next_y=1163.954132    Next_angle=-15.000000\n",
      "**********************************************************\n",
      "Current_x=1177.638690    Current_y=1163.954132   Current_angle=-15.000000\n",
      "3\n",
      "Next_x=1177.638690    Next_y=1163.954132    Next_angle=-75.000000\n",
      "**********************************************************\n",
      "Current_x=1177.638690    Current_y=1163.954132   Current_angle=-75.000000\n",
      "1\n",
      "Next_x=1178.829257    Next_y=1159.510873    Next_angle=-75.000000\n",
      "**********************************************************\n",
      "Current_x=1178.829257    Current_y=1159.510873   Current_angle=-75.000000\n",
      "4\n",
      "Next_x=1178.829257    Next_y=1159.510873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1178.829257    Current_y=1159.510873   Current_angle=-30.000000\n",
      "1\n",
      "Next_x=1182.812974    Next_y=1157.210873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1182.812974    Current_y=1157.210873   Current_angle=-30.000000\n",
      "1\n",
      "Next_x=1186.796691    Next_y=1154.910873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1186.796691    Current_y=1154.910873   Current_angle=-30.000000\n",
      "1\n",
      "Next_x=1190.780408    Next_y=1152.610873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1190.780408    Current_y=1152.610873   Current_angle=-30.000000\n",
      "1\n",
      "Next_x=1194.764125    Next_y=1150.310873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1194.764125    Current_y=1150.310873   Current_angle=-30.000000\n",
      "1\n",
      "Next_x=1198.747841    Next_y=1148.010873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1198.747841    Current_y=1148.010873   Current_angle=-30.000000\n",
      "3\n",
      "Next_x=1198.747841    Next_y=1148.010873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1198.747841    Current_y=1148.010873   Current_angle=-90.000000\n",
      "1\n",
      "Next_x=1198.747841    Next_y=1143.410873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1198.747841    Current_y=1143.410873   Current_angle=-90.000000\n",
      "1\n",
      "Next_x=1198.747841    Next_y=1138.810873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1198.747841    Current_y=1138.810873   Current_angle=-90.000000\n",
      "1\n",
      "Next_x=1198.747841    Next_y=1134.210873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1198.747841    Current_y=1134.210873   Current_angle=-90.000000\n",
      "1\n",
      "Next_x=1198.747841    Next_y=1129.610873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1198.747841    Current_y=1129.610873   Current_angle=-90.000000\n",
      "1\n",
      "Next_x=1198.747841    Next_y=1125.010873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1198.747841    Current_y=1125.010873   Current_angle=-90.000000\n",
      "1\n",
      "Next_x=1198.747841    Next_y=1120.410873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1198.747841    Current_y=1120.410873   Current_angle=-90.000000\n",
      "1\n",
      "Next_x=1198.747841    Next_y=1115.810873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1198.747841    Current_y=1115.810873   Current_angle=-90.000000\n",
      "1\n",
      "Next_x=1198.747841    Next_y=1111.210873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1198.747841    Current_y=1111.210873   Current_angle=-90.000000\n",
      "1\n",
      "Next_x=1198.747841    Next_y=1106.610873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1198.747841    Current_y=1106.610873   Current_angle=-90.000000\n",
      "1\n",
      "Next_x=1198.747841    Next_y=1102.010873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1198.747841    Current_y=1102.010873   Current_angle=-90.000000\n",
      "1\n",
      "Next_x=1198.747841    Next_y=1097.410873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1198.747841    Current_y=1097.410873   Current_angle=-90.000000\n",
      "1\n",
      "Next_x=1198.747841    Next_y=1092.810873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1198.747841    Current_y=1092.810873   Current_angle=-90.000000\n",
      "1\n",
      "Next_x=1198.747841    Next_y=1088.210873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1198.747841    Current_y=1088.210873   Current_angle=-90.000000\n",
      "1\n",
      "Next_x=1198.747841    Next_y=1083.610873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1198.747841    Current_y=1083.610873   Current_angle=-90.000000\n",
      "1\n",
      "Next_x=1198.747841    Next_y=1079.010873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1198.747841    Current_y=1079.010873   Current_angle=-90.000000\n",
      "1\n",
      "Next_x=1198.747841    Next_y=1074.410873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1198.747841    Current_y=1074.410873   Current_angle=-90.000000\n",
      "1\n",
      "Next_x=1198.747841    Next_y=1069.810873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1198.747841    Current_y=1069.810873   Current_angle=-90.000000\n",
      "1\n",
      "Next_x=1198.747841    Next_y=1065.210873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1198.747841    Current_y=1065.210873   Current_angle=-90.000000\n",
      "1\n",
      "Next_x=1198.747841    Next_y=1060.610873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1198.747841    Current_y=1060.610873   Current_angle=-90.000000\n",
      "1\n",
      "Next_x=1198.747841    Next_y=1056.010873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1198.747841    Current_y=1056.010873   Current_angle=-90.000000\n",
      "1\n",
      "Next_x=1198.747841    Next_y=1051.410873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1198.747841    Current_y=1051.410873   Current_angle=-90.000000\n",
      "1\n",
      "Next_x=1198.747841    Next_y=1046.810873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1198.747841    Current_y=1046.810873   Current_angle=-90.000000\n",
      "1\n",
      "Next_x=1198.747841    Next_y=1042.210873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1198.747841    Current_y=1042.210873   Current_angle=-90.000000\n",
      "1\n",
      "Next_x=1198.747841    Next_y=1037.610873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1198.747841    Current_y=1037.610873   Current_angle=-90.000000\n",
      "1\n",
      "Next_x=1198.747841    Next_y=1033.010873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1198.747841    Current_y=1033.010873   Current_angle=-90.000000\n",
      "1\n",
      "Next_x=1198.747841    Next_y=1028.410873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1198.747841    Current_y=1028.410873   Current_angle=-90.000000\n",
      "1\n",
      "Next_x=1198.747841    Next_y=1023.810873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1198.747841    Current_y=1023.810873   Current_angle=-90.000000\n",
      "0\n",
      "Next_x=1198.747841    Next_y=1012.250873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1198.747841    Current_y=1012.250873   Current_angle=-90.000000\n",
      "0\n",
      "Next_x=1198.747841    Next_y=1000.690873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1198.747841    Current_y=1000.690873   Current_angle=-90.000000\n",
      "0\n",
      "Next_x=1198.747841    Next_y=989.130873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1198.747841    Current_y=989.130873   Current_angle=-90.000000\n",
      "0\n",
      "Next_x=1198.747841    Next_y=977.570873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1198.747841    Current_y=977.570873   Current_angle=-90.000000\n",
      "0\n",
      "Next_x=1198.747841    Next_y=966.010873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1198.747841    Current_y=966.010873   Current_angle=-90.000000\n",
      "0\n",
      "Next_x=1198.747841    Next_y=954.450873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1198.747841    Current_y=954.450873   Current_angle=-90.000000\n",
      "0\n",
      "Next_x=1198.747841    Next_y=942.890873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1198.747841    Current_y=942.890873   Current_angle=-90.000000\n",
      "0\n",
      "Next_x=1198.747841    Next_y=931.330873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1198.747841    Current_y=931.330873   Current_angle=-90.000000\n",
      "0\n",
      "Next_x=1198.747841    Next_y=919.770873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1198.747841    Current_y=919.770873   Current_angle=-90.000000\n",
      "0\n",
      "Next_x=1198.747841    Next_y=908.210873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1198.747841    Current_y=908.210873   Current_angle=-90.000000\n",
      "0\n",
      "Next_x=1198.747841    Next_y=896.650873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1198.747841    Current_y=896.650873   Current_angle=-90.000000\n",
      "0\n",
      "Next_x=1198.747841    Next_y=885.090873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1198.747841    Current_y=885.090873   Current_angle=-90.000000\n",
      "0\n",
      "Next_x=1198.747841    Next_y=873.530873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1198.747841    Current_y=873.530873   Current_angle=-90.000000\n",
      "0\n",
      "Next_x=1198.747841    Next_y=861.970873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1198.747841    Current_y=861.970873   Current_angle=-90.000000\n",
      "0\n",
      "Next_x=1198.747841    Next_y=850.410873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1198.747841    Current_y=850.410873   Current_angle=-90.000000\n",
      "0\n",
      "Next_x=1198.747841    Next_y=838.850873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1198.747841    Current_y=838.850873   Current_angle=-90.000000\n",
      "0\n",
      "Next_x=1198.747841    Next_y=827.290873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1198.747841    Current_y=827.290873   Current_angle=-90.000000\n",
      "0\n",
      "Next_x=1198.747841    Next_y=815.730873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1198.747841    Current_y=815.730873   Current_angle=-90.000000\n",
      "0\n",
      "Next_x=1198.747841    Next_y=804.170873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1198.747841    Current_y=804.170873   Current_angle=-90.000000\n",
      "0\n",
      "Next_x=1198.747841    Next_y=792.610873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1198.747841    Current_y=792.610873   Current_angle=-90.000000\n",
      "0\n",
      "Next_x=1198.747841    Next_y=781.050873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1198.747841    Current_y=781.050873   Current_angle=-90.000000\n",
      "0\n",
      "Next_x=1198.747841    Next_y=769.490873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1198.747841    Current_y=769.490873   Current_angle=-90.000000\n",
      "4\n",
      "Next_x=1198.747841    Next_y=769.490873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1198.747841    Current_y=769.490873   Current_angle=-30.000000\n",
      "0\n",
      "Next_x=1208.759095    Next_y=763.710873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1208.759095    Current_y=763.710873   Current_angle=-30.000000\n",
      "0\n",
      "Next_x=1218.770349    Next_y=757.930873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1218.770349    Current_y=757.930873   Current_angle=-30.000000\n",
      "0\n",
      "Next_x=1228.781602    Next_y=752.150873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1228.781602    Current_y=752.150873   Current_angle=-30.000000\n",
      "0\n",
      "Next_x=1238.792856    Next_y=746.370873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1238.792856    Current_y=746.370873   Current_angle=-30.000000\n",
      "0\n",
      "Next_x=1248.804110    Next_y=740.590873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1248.804110    Current_y=740.590873   Current_angle=-30.000000\n",
      "0\n",
      "Next_x=1258.815363    Next_y=734.810873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1258.815363    Current_y=734.810873   Current_angle=-30.000000\n",
      "0\n",
      "Next_x=1268.826617    Next_y=729.030873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1268.826617    Current_y=729.030873   Current_angle=-30.000000\n",
      "0\n",
      "Next_x=1278.837871    Next_y=723.250873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1278.837871    Current_y=723.250873   Current_angle=-30.000000\n",
      "0\n",
      "Next_x=1288.849124    Next_y=717.470873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1288.849124    Current_y=717.470873   Current_angle=-30.000000\n",
      "0\n",
      "Next_x=1298.860378    Next_y=711.690873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1298.860378    Current_y=711.690873   Current_angle=-30.000000\n",
      "3\n",
      "Next_x=1298.860378    Next_y=711.690873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1298.860378    Current_y=711.690873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1298.860378    Next_y=707.850873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1298.860378    Current_y=707.850873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1298.860378    Next_y=704.010873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1298.860378    Current_y=704.010873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1298.860378    Next_y=700.170873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1298.860378    Current_y=700.170873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1298.860378    Next_y=696.330873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1298.860378    Current_y=696.330873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1298.860378    Next_y=692.490873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1298.860378    Current_y=692.490873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1298.860378    Next_y=688.650873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1298.860378    Current_y=688.650873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1298.860378    Next_y=684.810873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1298.860378    Current_y=684.810873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1298.860378    Next_y=680.970873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1298.860378    Current_y=680.970873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1298.860378    Next_y=677.130873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1298.860378    Current_y=677.130873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1298.860378    Next_y=673.290873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1298.860378    Current_y=673.290873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1298.860378    Next_y=669.450873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1298.860378    Current_y=669.450873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1298.860378    Next_y=665.610873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1298.860378    Current_y=665.610873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1298.860378    Next_y=661.770873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1298.860378    Current_y=661.770873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1298.860378    Next_y=657.930873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1298.860378    Current_y=657.930873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1298.860378    Next_y=654.090873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1298.860378    Current_y=654.090873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1298.860378    Next_y=650.250873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1298.860378    Current_y=650.250873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1298.860378    Next_y=646.410873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1298.860378    Current_y=646.410873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1298.860378    Next_y=642.570873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1298.860378    Current_y=642.570873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1298.860378    Next_y=638.730873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1298.860378    Current_y=638.730873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1298.860378    Next_y=634.890873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1298.860378    Current_y=634.890873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1298.860378    Next_y=631.050873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1298.860378    Current_y=631.050873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1298.860378    Next_y=627.210873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1298.860378    Current_y=627.210873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1298.860378    Next_y=623.370873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1298.860378    Current_y=623.370873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1298.860378    Next_y=619.530873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1298.860378    Current_y=619.530873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1298.860378    Next_y=615.690873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1298.860378    Current_y=615.690873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1298.860378    Next_y=611.850873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1298.860378    Current_y=611.850873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1298.860378    Next_y=608.010873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1298.860378    Current_y=608.010873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1298.860378    Next_y=604.170873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1298.860378    Current_y=604.170873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1298.860378    Next_y=600.330873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1298.860378    Current_y=600.330873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1298.860378    Next_y=596.490873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1298.860378    Current_y=596.490873   Current_angle=-90.000000\n",
      "4\n",
      "Next_x=1298.860378    Next_y=596.490873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1298.860378    Current_y=596.490873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1302.185916    Next_y=594.570873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1302.185916    Current_y=594.570873   Current_angle=-30.000000\n",
      "3\n",
      "Next_x=1302.185916    Next_y=594.570873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1302.185916    Current_y=594.570873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1302.185916    Next_y=590.730873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1302.185916    Current_y=590.730873   Current_angle=-90.000000\n",
      "4\n",
      "Next_x=1302.185916    Next_y=590.730873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1302.185916    Current_y=590.730873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1305.511453    Next_y=588.810873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1305.511453    Current_y=588.810873   Current_angle=-30.000000\n",
      "3\n",
      "Next_x=1305.511453    Next_y=588.810873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1305.511453    Current_y=588.810873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1305.511453    Next_y=584.970873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1305.511453    Current_y=584.970873   Current_angle=-90.000000\n",
      "4\n",
      "Next_x=1305.511453    Next_y=584.970873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1305.511453    Current_y=584.970873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1308.836991    Next_y=583.050873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1308.836991    Current_y=583.050873   Current_angle=-30.000000\n",
      "3\n",
      "Next_x=1308.836991    Next_y=583.050873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1308.836991    Current_y=583.050873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1308.836991    Next_y=579.210873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1308.836991    Current_y=579.210873   Current_angle=-90.000000\n",
      "4\n",
      "Next_x=1308.836991    Next_y=579.210873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1308.836991    Current_y=579.210873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1312.162528    Next_y=577.290873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1312.162528    Current_y=577.290873   Current_angle=-30.000000\n",
      "3\n",
      "Next_x=1312.162528    Next_y=577.290873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1312.162528    Current_y=577.290873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1312.162528    Next_y=573.450873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1312.162528    Current_y=573.450873   Current_angle=-90.000000\n",
      "4\n",
      "Next_x=1312.162528    Next_y=573.450873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1312.162528    Current_y=573.450873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1315.488066    Next_y=571.530873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1315.488066    Current_y=571.530873   Current_angle=-30.000000\n",
      "3\n",
      "Next_x=1315.488066    Next_y=571.530873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1315.488066    Current_y=571.530873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1315.488066    Next_y=567.690873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1315.488066    Current_y=567.690873   Current_angle=-90.000000\n",
      "4\n",
      "Next_x=1315.488066    Next_y=567.690873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1315.488066    Current_y=567.690873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1318.813603    Next_y=565.770873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1318.813603    Current_y=565.770873   Current_angle=-30.000000\n",
      "3\n",
      "Next_x=1318.813603    Next_y=565.770873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1318.813603    Current_y=565.770873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1318.813603    Next_y=561.930873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1318.813603    Current_y=561.930873   Current_angle=-90.000000\n",
      "4\n",
      "Next_x=1318.813603    Next_y=561.930873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1318.813603    Current_y=561.930873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1322.139141    Next_y=560.010873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1322.139141    Current_y=560.010873   Current_angle=-30.000000\n",
      "3\n",
      "Next_x=1322.139141    Next_y=560.010873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1322.139141    Current_y=560.010873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1322.139141    Next_y=556.170873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1322.139141    Current_y=556.170873   Current_angle=-90.000000\n",
      "4\n",
      "Next_x=1322.139141    Next_y=556.170873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1322.139141    Current_y=556.170873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1325.464679    Next_y=554.250873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1325.464679    Current_y=554.250873   Current_angle=-30.000000\n",
      "3\n",
      "Next_x=1325.464679    Next_y=554.250873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1325.464679    Current_y=554.250873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1325.464679    Next_y=550.410873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1325.464679    Current_y=550.410873   Current_angle=-90.000000\n",
      "4\n",
      "Next_x=1325.464679    Next_y=550.410873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1325.464679    Current_y=550.410873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1328.790216    Next_y=548.490873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1328.790216    Current_y=548.490873   Current_angle=-30.000000\n",
      "3\n",
      "Next_x=1328.790216    Next_y=548.490873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1328.790216    Current_y=548.490873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1328.790216    Next_y=544.650873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1328.790216    Current_y=544.650873   Current_angle=-90.000000\n",
      "4\n",
      "Next_x=1328.790216    Next_y=544.650873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1328.790216    Current_y=544.650873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1332.115754    Next_y=542.730873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1332.115754    Current_y=542.730873   Current_angle=-30.000000\n",
      "3\n",
      "Next_x=1332.115754    Next_y=542.730873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1332.115754    Current_y=542.730873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1332.115754    Next_y=538.890873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1332.115754    Current_y=538.890873   Current_angle=-90.000000\n",
      "4\n",
      "Next_x=1332.115754    Next_y=538.890873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1332.115754    Current_y=538.890873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1335.441291    Next_y=536.970873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1335.441291    Current_y=536.970873   Current_angle=-30.000000\n",
      "3\n",
      "Next_x=1335.441291    Next_y=536.970873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1335.441291    Current_y=536.970873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1335.441291    Next_y=533.130873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1335.441291    Current_y=533.130873   Current_angle=-90.000000\n",
      "4\n",
      "Next_x=1335.441291    Next_y=533.130873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1335.441291    Current_y=533.130873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1338.766829    Next_y=531.210873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1338.766829    Current_y=531.210873   Current_angle=-30.000000\n",
      "3\n",
      "Next_x=1338.766829    Next_y=531.210873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1338.766829    Current_y=531.210873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1338.766829    Next_y=527.370873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1338.766829    Current_y=527.370873   Current_angle=-90.000000\n",
      "4\n",
      "Next_x=1338.766829    Next_y=527.370873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1338.766829    Current_y=527.370873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1342.092366    Next_y=525.450873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1342.092366    Current_y=525.450873   Current_angle=-30.000000\n",
      "3\n",
      "Next_x=1342.092366    Next_y=525.450873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1342.092366    Current_y=525.450873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1342.092366    Next_y=521.610873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1342.092366    Current_y=521.610873   Current_angle=-90.000000\n",
      "4\n",
      "Next_x=1342.092366    Next_y=521.610873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1342.092366    Current_y=521.610873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1345.417904    Next_y=519.690873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1345.417904    Current_y=519.690873   Current_angle=-30.000000\n",
      "3\n",
      "Next_x=1345.417904    Next_y=519.690873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1345.417904    Current_y=519.690873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1345.417904    Next_y=515.850873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1345.417904    Current_y=515.850873   Current_angle=-90.000000\n",
      "4\n",
      "Next_x=1345.417904    Next_y=515.850873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1345.417904    Current_y=515.850873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1348.743441    Next_y=513.930873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1348.743441    Current_y=513.930873   Current_angle=-30.000000\n",
      "3\n",
      "Next_x=1348.743441    Next_y=513.930873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1348.743441    Current_y=513.930873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1348.743441    Next_y=510.090873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1348.743441    Current_y=510.090873   Current_angle=-90.000000\n",
      "4\n",
      "Next_x=1348.743441    Next_y=510.090873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1348.743441    Current_y=510.090873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1352.068979    Next_y=508.170873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1352.068979    Current_y=508.170873   Current_angle=-30.000000\n",
      "3\n",
      "Next_x=1352.068979    Next_y=508.170873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1352.068979    Current_y=508.170873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1352.068979    Next_y=504.330873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1352.068979    Current_y=504.330873   Current_angle=-90.000000\n",
      "4\n",
      "Next_x=1352.068979    Next_y=504.330873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1352.068979    Current_y=504.330873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1355.394516    Next_y=502.410873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1355.394516    Current_y=502.410873   Current_angle=-30.000000\n",
      "3\n",
      "Next_x=1355.394516    Next_y=502.410873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1355.394516    Current_y=502.410873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1355.394516    Next_y=498.570873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1355.394516    Current_y=498.570873   Current_angle=-90.000000\n",
      "4\n",
      "Next_x=1355.394516    Next_y=498.570873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1355.394516    Current_y=498.570873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1358.720054    Next_y=496.650873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1358.720054    Current_y=496.650873   Current_angle=-30.000000\n",
      "3\n",
      "Next_x=1358.720054    Next_y=496.650873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1358.720054    Current_y=496.650873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1358.720054    Next_y=492.810873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1358.720054    Current_y=492.810873   Current_angle=-90.000000\n",
      "4\n",
      "Next_x=1358.720054    Next_y=492.810873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1358.720054    Current_y=492.810873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1362.045592    Next_y=490.890873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1362.045592    Current_y=490.890873   Current_angle=-30.000000\n",
      "3\n",
      "Next_x=1362.045592    Next_y=490.890873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1362.045592    Current_y=490.890873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1362.045592    Next_y=487.050873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1362.045592    Current_y=487.050873   Current_angle=-90.000000\n",
      "4\n",
      "Next_x=1362.045592    Next_y=487.050873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1362.045592    Current_y=487.050873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1365.371129    Next_y=485.130873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1365.371129    Current_y=485.130873   Current_angle=-30.000000\n",
      "3\n",
      "Next_x=1365.371129    Next_y=485.130873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1365.371129    Current_y=485.130873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1365.371129    Next_y=481.290873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1365.371129    Current_y=481.290873   Current_angle=-90.000000\n",
      "4\n",
      "Next_x=1365.371129    Next_y=481.290873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1365.371129    Current_y=481.290873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1368.696667    Next_y=479.370873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1368.696667    Current_y=479.370873   Current_angle=-30.000000\n",
      "3\n",
      "Next_x=1368.696667    Next_y=479.370873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1368.696667    Current_y=479.370873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1368.696667    Next_y=475.530873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1368.696667    Current_y=475.530873   Current_angle=-90.000000\n",
      "4\n",
      "Next_x=1368.696667    Next_y=475.530873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1368.696667    Current_y=475.530873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1372.022204    Next_y=473.610873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1372.022204    Current_y=473.610873   Current_angle=-30.000000\n",
      "3\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Next_x=1372.022204    Next_y=473.610873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1372.022204    Current_y=473.610873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1372.022204    Next_y=469.770873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1372.022204    Current_y=469.770873   Current_angle=-90.000000\n",
      "4\n",
      "Next_x=1372.022204    Next_y=469.770873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1372.022204    Current_y=469.770873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1375.347742    Next_y=467.850873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1375.347742    Current_y=467.850873   Current_angle=-30.000000\n",
      "3\n",
      "Next_x=1375.347742    Next_y=467.850873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1375.347742    Current_y=467.850873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1375.347742    Next_y=464.010873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1375.347742    Current_y=464.010873   Current_angle=-90.000000\n",
      "4\n",
      "Next_x=1375.347742    Next_y=464.010873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1375.347742    Current_y=464.010873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1378.673279    Next_y=462.090873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1378.673279    Current_y=462.090873   Current_angle=-30.000000\n",
      "3\n",
      "Next_x=1378.673279    Next_y=462.090873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1378.673279    Current_y=462.090873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1378.673279    Next_y=458.250873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1378.673279    Current_y=458.250873   Current_angle=-90.000000\n",
      "4\n",
      "Next_x=1378.673279    Next_y=458.250873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1378.673279    Current_y=458.250873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1381.998817    Next_y=456.330873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1381.998817    Current_y=456.330873   Current_angle=-30.000000\n",
      "3\n",
      "Next_x=1381.998817    Next_y=456.330873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1381.998817    Current_y=456.330873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1381.998817    Next_y=452.490873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1381.998817    Current_y=452.490873   Current_angle=-90.000000\n",
      "4\n",
      "Next_x=1381.998817    Next_y=452.490873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1381.998817    Current_y=452.490873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1385.324354    Next_y=450.570873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1385.324354    Current_y=450.570873   Current_angle=-30.000000\n",
      "3\n",
      "Next_x=1385.324354    Next_y=450.570873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1385.324354    Current_y=450.570873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1385.324354    Next_y=446.730873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1385.324354    Current_y=446.730873   Current_angle=-90.000000\n",
      "4\n",
      "Next_x=1385.324354    Next_y=446.730873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1385.324354    Current_y=446.730873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1388.649892    Next_y=444.810873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1388.649892    Current_y=444.810873   Current_angle=-30.000000\n",
      "3\n",
      "Next_x=1388.649892    Next_y=444.810873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1388.649892    Current_y=444.810873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1388.649892    Next_y=440.970873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1388.649892    Current_y=440.970873   Current_angle=-90.000000\n",
      "4\n",
      "Next_x=1388.649892    Next_y=440.970873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1388.649892    Current_y=440.970873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1391.975430    Next_y=439.050873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1391.975430    Current_y=439.050873   Current_angle=-30.000000\n",
      "3\n",
      "Next_x=1391.975430    Next_y=439.050873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1391.975430    Current_y=439.050873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1391.975430    Next_y=435.210873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1391.975430    Current_y=435.210873   Current_angle=-90.000000\n",
      "4\n",
      "Next_x=1391.975430    Next_y=435.210873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1391.975430    Current_y=435.210873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1395.300967    Next_y=433.290873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1395.300967    Current_y=433.290873   Current_angle=-30.000000\n",
      "3\n",
      "Next_x=1395.300967    Next_y=433.290873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1395.300967    Current_y=433.290873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1395.300967    Next_y=429.450873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1395.300967    Current_y=429.450873   Current_angle=-90.000000\n",
      "4\n",
      "Next_x=1395.300967    Next_y=429.450873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1395.300967    Current_y=429.450873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1398.626505    Next_y=427.530873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1398.626505    Current_y=427.530873   Current_angle=-30.000000\n",
      "3\n",
      "Next_x=1398.626505    Next_y=427.530873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1398.626505    Current_y=427.530873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1398.626505    Next_y=423.690873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1398.626505    Current_y=423.690873   Current_angle=-90.000000\n",
      "4\n",
      "Next_x=1398.626505    Next_y=423.690873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1398.626505    Current_y=423.690873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1401.952042    Next_y=421.770873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1401.952042    Current_y=421.770873   Current_angle=-30.000000\n",
      "3\n",
      "Next_x=1401.952042    Next_y=421.770873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1401.952042    Current_y=421.770873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1401.952042    Next_y=417.930873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1401.952042    Current_y=417.930873   Current_angle=-90.000000\n",
      "4\n",
      "Next_x=1401.952042    Next_y=417.930873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1401.952042    Current_y=417.930873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1405.277580    Next_y=416.010873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1405.277580    Current_y=416.010873   Current_angle=-30.000000\n",
      "3\n",
      "Next_x=1405.277580    Next_y=416.010873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1405.277580    Current_y=416.010873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1405.277580    Next_y=412.170873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1405.277580    Current_y=412.170873   Current_angle=-90.000000\n",
      "4\n",
      "Next_x=1405.277580    Next_y=412.170873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1405.277580    Current_y=412.170873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1408.603117    Next_y=410.250873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1408.603117    Current_y=410.250873   Current_angle=-30.000000\n",
      "3\n",
      "Next_x=1408.603117    Next_y=410.250873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1408.603117    Current_y=410.250873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1408.603117    Next_y=406.410873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1408.603117    Current_y=406.410873   Current_angle=-90.000000\n",
      "4\n",
      "Next_x=1408.603117    Next_y=406.410873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1408.603117    Current_y=406.410873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1411.928655    Next_y=404.490873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1411.928655    Current_y=404.490873   Current_angle=-30.000000\n",
      "3\n",
      "Next_x=1411.928655    Next_y=404.490873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1411.928655    Current_y=404.490873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1411.928655    Next_y=400.650873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1411.928655    Current_y=400.650873   Current_angle=-90.000000\n",
      "4\n",
      "Next_x=1411.928655    Next_y=400.650873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1411.928655    Current_y=400.650873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1415.254192    Next_y=398.730873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1415.254192    Current_y=398.730873   Current_angle=-30.000000\n",
      "3\n",
      "Next_x=1415.254192    Next_y=398.730873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1415.254192    Current_y=398.730873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1415.254192    Next_y=394.890873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1415.254192    Current_y=394.890873   Current_angle=-90.000000\n",
      "4\n",
      "Next_x=1415.254192    Next_y=394.890873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1415.254192    Current_y=394.890873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1418.579730    Next_y=392.970873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1418.579730    Current_y=392.970873   Current_angle=-30.000000\n",
      "3\n",
      "Next_x=1418.579730    Next_y=392.970873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1418.579730    Current_y=392.970873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1418.579730    Next_y=389.130873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1418.579730    Current_y=389.130873   Current_angle=-90.000000\n",
      "4\n",
      "Next_x=1418.579730    Next_y=389.130873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1418.579730    Current_y=389.130873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1421.905267    Next_y=387.210873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1421.905267    Current_y=387.210873   Current_angle=-30.000000\n",
      "3\n",
      "Next_x=1421.905267    Next_y=387.210873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1421.905267    Current_y=387.210873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1421.905267    Next_y=383.370873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1421.905267    Current_y=383.370873   Current_angle=-90.000000\n",
      "4\n",
      "Next_x=1421.905267    Next_y=383.370873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1421.905267    Current_y=383.370873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1425.230805    Next_y=381.450873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1425.230805    Current_y=381.450873   Current_angle=-30.000000\n",
      "3\n",
      "Next_x=1425.230805    Next_y=381.450873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1425.230805    Current_y=381.450873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1425.230805    Next_y=377.610873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1425.230805    Current_y=377.610873   Current_angle=-90.000000\n",
      "4\n",
      "Next_x=1425.230805    Next_y=377.610873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1425.230805    Current_y=377.610873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1428.556343    Next_y=375.690873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1428.556343    Current_y=375.690873   Current_angle=-30.000000\n",
      "3\n",
      "Next_x=1428.556343    Next_y=375.690873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1428.556343    Current_y=375.690873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1428.556343    Next_y=371.850873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1428.556343    Current_y=371.850873   Current_angle=-90.000000\n",
      "4\n",
      "Next_x=1428.556343    Next_y=371.850873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1428.556343    Current_y=371.850873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1431.881880    Next_y=369.930873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1431.881880    Current_y=369.930873   Current_angle=-30.000000\n",
      "3\n",
      "Next_x=1431.881880    Next_y=369.930873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1431.881880    Current_y=369.930873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1431.881880    Next_y=366.090873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1431.881880    Current_y=366.090873   Current_angle=-90.000000\n",
      "4\n",
      "Next_x=1431.881880    Next_y=366.090873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1431.881880    Current_y=366.090873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1435.207418    Next_y=364.170873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1435.207418    Current_y=364.170873   Current_angle=-30.000000\n",
      "3\n",
      "Next_x=1435.207418    Next_y=364.170873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1435.207418    Current_y=364.170873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1435.207418    Next_y=360.330873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1435.207418    Current_y=360.330873   Current_angle=-90.000000\n",
      "4\n",
      "Next_x=1435.207418    Next_y=360.330873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1435.207418    Current_y=360.330873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1438.532955    Next_y=358.410873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1438.532955    Current_y=358.410873   Current_angle=-30.000000\n",
      "3\n",
      "Next_x=1438.532955    Next_y=358.410873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1438.532955    Current_y=358.410873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1438.532955    Next_y=354.570873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1438.532955    Current_y=354.570873   Current_angle=-90.000000\n",
      "4\n",
      "Next_x=1438.532955    Next_y=354.570873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1438.532955    Current_y=354.570873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1441.858493    Next_y=352.650873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1441.858493    Current_y=352.650873   Current_angle=-30.000000\n",
      "3\n",
      "Next_x=1441.858493    Next_y=352.650873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1441.858493    Current_y=352.650873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1441.858493    Next_y=348.810873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1441.858493    Current_y=348.810873   Current_angle=-90.000000\n",
      "4\n",
      "Next_x=1441.858493    Next_y=348.810873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1441.858493    Current_y=348.810873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1445.184030    Next_y=346.890873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1445.184030    Current_y=346.890873   Current_angle=-30.000000\n",
      "3\n",
      "Next_x=1445.184030    Next_y=346.890873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1445.184030    Current_y=346.890873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1445.184030    Next_y=343.050873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1445.184030    Current_y=343.050873   Current_angle=-90.000000\n",
      "4\n",
      "Next_x=1445.184030    Next_y=343.050873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1445.184030    Current_y=343.050873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1448.509568    Next_y=341.130873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1448.509568    Current_y=341.130873   Current_angle=-30.000000\n",
      "3\n",
      "Next_x=1448.509568    Next_y=341.130873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1448.509568    Current_y=341.130873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1448.509568    Next_y=337.290873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1448.509568    Current_y=337.290873   Current_angle=-90.000000\n",
      "4\n",
      "Next_x=1448.509568    Next_y=337.290873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1448.509568    Current_y=337.290873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1451.835105    Next_y=335.370873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1451.835105    Current_y=335.370873   Current_angle=-30.000000\n",
      "3\n",
      "Next_x=1451.835105    Next_y=335.370873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1451.835105    Current_y=335.370873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1451.835105    Next_y=331.530873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1451.835105    Current_y=331.530873   Current_angle=-90.000000\n",
      "4\n",
      "Next_x=1451.835105    Next_y=331.530873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1451.835105    Current_y=331.530873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1455.160643    Next_y=329.610873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1455.160643    Current_y=329.610873   Current_angle=-30.000000\n",
      "3\n",
      "Next_x=1455.160643    Next_y=329.610873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1455.160643    Current_y=329.610873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1455.160643    Next_y=325.770873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1455.160643    Current_y=325.770873   Current_angle=-90.000000\n",
      "4\n",
      "Next_x=1455.160643    Next_y=325.770873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1455.160643    Current_y=325.770873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1458.486181    Next_y=323.850873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1458.486181    Current_y=323.850873   Current_angle=-30.000000\n",
      "3\n",
      "Next_x=1458.486181    Next_y=323.850873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1458.486181    Current_y=323.850873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1458.486181    Next_y=320.010873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1458.486181    Current_y=320.010873   Current_angle=-90.000000\n",
      "4\n",
      "Next_x=1458.486181    Next_y=320.010873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1458.486181    Current_y=320.010873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1461.811718    Next_y=318.090873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1461.811718    Current_y=318.090873   Current_angle=-30.000000\n",
      "3\n",
      "Next_x=1461.811718    Next_y=318.090873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1461.811718    Current_y=318.090873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1461.811718    Next_y=314.250873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1461.811718    Current_y=314.250873   Current_angle=-90.000000\n",
      "4\n",
      "Next_x=1461.811718    Next_y=314.250873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1461.811718    Current_y=314.250873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1465.137256    Next_y=312.330873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1465.137256    Current_y=312.330873   Current_angle=-30.000000\n",
      "3\n",
      "Next_x=1465.137256    Next_y=312.330873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1465.137256    Current_y=312.330873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1465.137256    Next_y=308.490873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1465.137256    Current_y=308.490873   Current_angle=-90.000000\n",
      "4\n",
      "Next_x=1465.137256    Next_y=308.490873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1465.137256    Current_y=308.490873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1468.462793    Next_y=306.570873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1468.462793    Current_y=306.570873   Current_angle=-30.000000\n",
      "3\n",
      "Next_x=1468.462793    Next_y=306.570873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1468.462793    Current_y=306.570873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1468.462793    Next_y=302.730873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1468.462793    Current_y=302.730873   Current_angle=-90.000000\n",
      "4\n",
      "Next_x=1468.462793    Next_y=302.730873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1468.462793    Current_y=302.730873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1471.788331    Next_y=300.810873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1471.788331    Current_y=300.810873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1475.113868    Next_y=298.890873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1475.113868    Current_y=298.890873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1478.439406    Next_y=296.970873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1478.439406    Current_y=296.970873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1481.764943    Next_y=295.050873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1481.764943    Current_y=295.050873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1485.090481    Next_y=293.130873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1485.090481    Current_y=293.130873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1488.416019    Next_y=291.210873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1488.416019    Current_y=291.210873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1491.741556    Next_y=289.290873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1491.741556    Current_y=289.290873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1495.067094    Next_y=287.370873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1495.067094    Current_y=287.370873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1498.392631    Next_y=285.450873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1498.392631    Current_y=285.450873   Current_angle=-30.000000\n",
      "2\n",
      "Next_x=1501.718169    Next_y=283.530873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=283.530873   Current_angle=-30.000000\n",
      "3\n",
      "Next_x=1501.718169    Next_y=283.530873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=283.530873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1501.718169    Next_y=279.690873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=279.690873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1501.718169    Next_y=275.850873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=275.850873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1501.718169    Next_y=272.010873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=272.010873   Current_angle=-90.000000\n",
      "2\n",
      "Next_x=1501.718169    Next_y=268.170873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=268.170873   Current_angle=-90.000000\n",
      "0\n",
      "Next_x=1501.718169    Next_y=256.610873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=256.610873   Current_angle=-90.000000\n",
      "0\n",
      "Next_x=1501.718169    Next_y=245.050873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=245.050873   Current_angle=-90.000000\n",
      "3\n",
      "Next_x=1501.718169    Next_y=245.050873    Next_angle=-150.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=245.050873   Current_angle=-150.000000\n",
      "3\n",
      "Next_x=1501.718169    Next_y=245.050873    Next_angle=-210.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=245.050873   Current_angle=-210.000000\n",
      "3\n",
      "Next_x=1501.718169    Next_y=245.050873    Next_angle=-270.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=245.050873   Current_angle=-270.000000\n",
      "0\n",
      "Next_x=1501.718169    Next_y=256.610873    Next_angle=-270.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=256.610873   Current_angle=-270.000000\n",
      "4\n",
      "Next_x=1501.718169    Next_y=256.610873    Next_angle=-210.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=256.610873   Current_angle=-210.000000\n",
      "4\n",
      "Next_x=1501.718169    Next_y=256.610873    Next_angle=-150.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=256.610873   Current_angle=-150.000000\n",
      "0\n",
      "Next_x=1491.706915    Next_y=250.830873    Next_angle=-150.000000\n",
      "**********************************************************\n",
      "Current_x=1491.706915    Current_y=250.830873   Current_angle=-150.000000\n",
      "4\n",
      "Next_x=1491.706915    Next_y=250.830873    Next_angle=-90.000000\n",
      "**********************************************************\n",
      "Current_x=1491.706915    Current_y=250.830873   Current_angle=-90.000000\n",
      "4\n",
      "Next_x=1491.706915    Next_y=250.830873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1491.706915    Current_y=250.830873   Current_angle=-30.000000\n",
      "0\n",
      "Next_x=1501.718169    Next_y=245.050873    Next_angle=-30.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=245.050873   Current_angle=-30.000000\n",
      "4\n",
      "Next_x=1501.718169    Next_y=245.050873    Next_angle=30.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=245.050873   Current_angle=30.000000\n",
      "4\n",
      "Next_x=1501.718169    Next_y=245.050873    Next_angle=90.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=245.050873   Current_angle=90.000000\n",
      "0\n",
      "Next_x=1501.718169    Next_y=256.610873    Next_angle=90.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=256.610873   Current_angle=90.000000\n",
      "4\n",
      "Next_x=1501.718169    Next_y=256.610873    Next_angle=150.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=256.610873   Current_angle=150.000000\n",
      "4\n",
      "Next_x=1501.718169    Next_y=256.610873    Next_angle=210.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=256.610873   Current_angle=210.000000\n",
      "0\n",
      "Next_x=1491.706915    Next_y=250.830873    Next_angle=210.000000\n",
      "**********************************************************\n",
      "Current_x=1491.706915    Current_y=250.830873   Current_angle=210.000000\n",
      "4\n",
      "Next_x=1491.706915    Next_y=250.830873    Next_angle=270.000000\n",
      "**********************************************************\n",
      "Current_x=1491.706915    Current_y=250.830873   Current_angle=270.000000\n",
      "4\n",
      "Next_x=1491.706915    Next_y=250.830873    Next_angle=330.000000\n",
      "**********************************************************\n",
      "Current_x=1491.706915    Current_y=250.830873   Current_angle=330.000000\n",
      "0\n",
      "Next_x=1501.718169    Next_y=245.050873    Next_angle=330.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=245.050873   Current_angle=330.000000\n",
      "4\n",
      "Next_x=1501.718169    Next_y=245.050873    Next_angle=390.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=245.050873   Current_angle=390.000000\n",
      "4\n",
      "Next_x=1501.718169    Next_y=245.050873    Next_angle=450.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=245.050873   Current_angle=450.000000\n",
      "0\n",
      "Next_x=1501.718169    Next_y=256.610873    Next_angle=450.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=256.610873   Current_angle=450.000000\n",
      "4\n",
      "Next_x=1501.718169    Next_y=256.610873    Next_angle=510.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=256.610873   Current_angle=510.000000\n",
      "4\n",
      "Next_x=1501.718169    Next_y=256.610873    Next_angle=570.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=256.610873   Current_angle=570.000000\n",
      "0\n",
      "Next_x=1491.706915    Next_y=250.830873    Next_angle=570.000000\n",
      "**********************************************************\n",
      "Current_x=1491.706915    Current_y=250.830873   Current_angle=570.000000\n",
      "4\n",
      "Next_x=1491.706915    Next_y=250.830873    Next_angle=630.000000\n",
      "**********************************************************\n",
      "Current_x=1491.706915    Current_y=250.830873   Current_angle=630.000000\n",
      "4\n",
      "Next_x=1491.706915    Next_y=250.830873    Next_angle=690.000000\n",
      "**********************************************************\n",
      "Current_x=1491.706915    Current_y=250.830873   Current_angle=690.000000\n",
      "0\n",
      "Next_x=1501.718169    Next_y=245.050873    Next_angle=690.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=245.050873   Current_angle=690.000000\n",
      "4\n",
      "Next_x=1501.718169    Next_y=245.050873    Next_angle=750.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=245.050873   Current_angle=750.000000\n",
      "4\n",
      "Next_x=1501.718169    Next_y=245.050873    Next_angle=810.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=245.050873   Current_angle=810.000000\n",
      "0\n",
      "Next_x=1501.718169    Next_y=256.610873    Next_angle=810.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=256.610873   Current_angle=810.000000\n",
      "4\n",
      "Next_x=1501.718169    Next_y=256.610873    Next_angle=870.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=256.610873   Current_angle=870.000000\n",
      "4\n",
      "Next_x=1501.718169    Next_y=256.610873    Next_angle=930.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=256.610873   Current_angle=930.000000\n",
      "0\n",
      "Next_x=1491.706915    Next_y=250.830873    Next_angle=930.000000\n",
      "**********************************************************\n",
      "Current_x=1491.706915    Current_y=250.830873   Current_angle=930.000000\n",
      "4\n",
      "Next_x=1491.706915    Next_y=250.830873    Next_angle=990.000000\n",
      "**********************************************************\n",
      "Current_x=1491.706915    Current_y=250.830873   Current_angle=990.000000\n",
      "4\n",
      "Next_x=1491.706915    Next_y=250.830873    Next_angle=1050.000000\n",
      "**********************************************************\n",
      "Current_x=1491.706915    Current_y=250.830873   Current_angle=1050.000000\n",
      "0\n",
      "Next_x=1501.718169    Next_y=245.050873    Next_angle=1050.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=245.050873   Current_angle=1050.000000\n",
      "4\n",
      "Next_x=1501.718169    Next_y=245.050873    Next_angle=1110.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=245.050873   Current_angle=1110.000000\n",
      "4\n",
      "Next_x=1501.718169    Next_y=245.050873    Next_angle=1170.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=245.050873   Current_angle=1170.000000\n",
      "0\n",
      "Next_x=1501.718169    Next_y=256.610873    Next_angle=1170.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=256.610873   Current_angle=1170.000000\n",
      "4\n",
      "Next_x=1501.718169    Next_y=256.610873    Next_angle=1230.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=256.610873   Current_angle=1230.000000\n",
      "4\n",
      "Next_x=1501.718169    Next_y=256.610873    Next_angle=1290.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=256.610873   Current_angle=1290.000000\n",
      "0\n",
      "Next_x=1491.706915    Next_y=250.830873    Next_angle=1290.000000\n",
      "**********************************************************\n",
      "Current_x=1491.706915    Current_y=250.830873   Current_angle=1290.000000\n",
      "4\n",
      "Next_x=1491.706915    Next_y=250.830873    Next_angle=1350.000000\n",
      "**********************************************************\n",
      "Current_x=1491.706915    Current_y=250.830873   Current_angle=1350.000000\n",
      "4\n",
      "Next_x=1491.706915    Next_y=250.830873    Next_angle=1410.000000\n",
      "**********************************************************\n",
      "Current_x=1491.706915    Current_y=250.830873   Current_angle=1410.000000\n",
      "0\n",
      "Next_x=1501.718169    Next_y=245.050873    Next_angle=1410.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=245.050873   Current_angle=1410.000000\n",
      "4\n",
      "Next_x=1501.718169    Next_y=245.050873    Next_angle=1470.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=245.050873   Current_angle=1470.000000\n",
      "4\n",
      "Next_x=1501.718169    Next_y=245.050873    Next_angle=1530.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=245.050873   Current_angle=1530.000000\n",
      "0\n",
      "Next_x=1501.718169    Next_y=256.610873    Next_angle=1530.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=256.610873   Current_angle=1530.000000\n",
      "4\n",
      "Next_x=1501.718169    Next_y=256.610873    Next_angle=1590.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=256.610873   Current_angle=1590.000000\n",
      "4\n",
      "Next_x=1501.718169    Next_y=256.610873    Next_angle=1650.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=256.610873   Current_angle=1650.000000\n",
      "0\n",
      "Next_x=1491.706915    Next_y=250.830873    Next_angle=1650.000000\n",
      "**********************************************************\n",
      "Current_x=1491.706915    Current_y=250.830873   Current_angle=1650.000000\n",
      "4\n",
      "Next_x=1491.706915    Next_y=250.830873    Next_angle=1710.000000\n",
      "**********************************************************\n",
      "Current_x=1491.706915    Current_y=250.830873   Current_angle=1710.000000\n",
      "4\n",
      "Next_x=1491.706915    Next_y=250.830873    Next_angle=1770.000000\n",
      "**********************************************************\n",
      "Current_x=1491.706915    Current_y=250.830873   Current_angle=1770.000000\n",
      "0\n",
      "Next_x=1501.718169    Next_y=245.050873    Next_angle=1770.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=245.050873   Current_angle=1770.000000\n",
      "4\n",
      "Next_x=1501.718169    Next_y=245.050873    Next_angle=1830.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=245.050873   Current_angle=1830.000000\n",
      "4\n",
      "Next_x=1501.718169    Next_y=245.050873    Next_angle=1890.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=245.050873   Current_angle=1890.000000\n",
      "0\n",
      "Next_x=1501.718169    Next_y=256.610873    Next_angle=1890.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=256.610873   Current_angle=1890.000000\n",
      "4\n",
      "Next_x=1501.718169    Next_y=256.610873    Next_angle=1950.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=256.610873   Current_angle=1950.000000\n",
      "4\n",
      "Next_x=1501.718169    Next_y=256.610873    Next_angle=2010.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=256.610873   Current_angle=2010.000000\n",
      "0\n",
      "Next_x=1491.706915    Next_y=250.830873    Next_angle=2010.000000\n",
      "**********************************************************\n",
      "Current_x=1491.706915    Current_y=250.830873   Current_angle=2010.000000\n",
      "4\n",
      "Next_x=1491.706915    Next_y=250.830873    Next_angle=2070.000000\n",
      "**********************************************************\n",
      "Current_x=1491.706915    Current_y=250.830873   Current_angle=2070.000000\n",
      "4\n",
      "Next_x=1491.706915    Next_y=250.830873    Next_angle=2130.000000\n",
      "**********************************************************\n",
      "Current_x=1491.706915    Current_y=250.830873   Current_angle=2130.000000\n",
      "0\n",
      "Next_x=1501.718169    Next_y=245.050873    Next_angle=2130.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=245.050873   Current_angle=2130.000000\n",
      "4\n",
      "Next_x=1501.718169    Next_y=245.050873    Next_angle=2190.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=245.050873   Current_angle=2190.000000\n",
      "4\n",
      "Next_x=1501.718169    Next_y=245.050873    Next_angle=2250.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=245.050873   Current_angle=2250.000000\n",
      "0\n",
      "Next_x=1501.718169    Next_y=256.610873    Next_angle=2250.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=256.610873   Current_angle=2250.000000\n",
      "4\n",
      "Next_x=1501.718169    Next_y=256.610873    Next_angle=2310.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=256.610873   Current_angle=2310.000000\n",
      "4\n",
      "Next_x=1501.718169    Next_y=256.610873    Next_angle=2370.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=256.610873   Current_angle=2370.000000\n",
      "0\n",
      "Next_x=1491.706915    Next_y=250.830873    Next_angle=2370.000000\n",
      "**********************************************************\n",
      "Current_x=1491.706915    Current_y=250.830873   Current_angle=2370.000000\n",
      "4\n",
      "Next_x=1491.706915    Next_y=250.830873    Next_angle=2430.000000\n",
      "**********************************************************\n",
      "Current_x=1491.706915    Current_y=250.830873   Current_angle=2430.000000\n",
      "4\n",
      "Next_x=1491.706915    Next_y=250.830873    Next_angle=2490.000000\n",
      "**********************************************************\n",
      "Current_x=1491.706915    Current_y=250.830873   Current_angle=2490.000000\n",
      "0\n",
      "Next_x=1501.718169    Next_y=245.050873    Next_angle=2490.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=245.050873   Current_angle=2490.000000\n",
      "4\n",
      "Next_x=1501.718169    Next_y=245.050873    Next_angle=2550.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=245.050873   Current_angle=2550.000000\n",
      "4\n",
      "Next_x=1501.718169    Next_y=245.050873    Next_angle=2610.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=245.050873   Current_angle=2610.000000\n",
      "0\n",
      "Next_x=1501.718169    Next_y=256.610873    Next_angle=2610.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=256.610873   Current_angle=2610.000000\n",
      "4\n",
      "Next_x=1501.718169    Next_y=256.610873    Next_angle=2670.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=256.610873   Current_angle=2670.000000\n",
      "4\n",
      "Next_x=1501.718169    Next_y=256.610873    Next_angle=2730.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=256.610873   Current_angle=2730.000000\n",
      "0\n",
      "Next_x=1491.706915    Next_y=250.830873    Next_angle=2730.000000\n",
      "**********************************************************\n",
      "Current_x=1491.706915    Current_y=250.830873   Current_angle=2730.000000\n",
      "4\n",
      "Next_x=1491.706915    Next_y=250.830873    Next_angle=2790.000000\n",
      "**********************************************************\n",
      "Current_x=1491.706915    Current_y=250.830873   Current_angle=2790.000000\n",
      "4\n",
      "Next_x=1491.706915    Next_y=250.830873    Next_angle=2850.000000\n",
      "**********************************************************\n",
      "Current_x=1491.706915    Current_y=250.830873   Current_angle=2850.000000\n",
      "0\n",
      "Next_x=1501.718169    Next_y=245.050873    Next_angle=2850.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=245.050873   Current_angle=2850.000000\n",
      "4\n",
      "Next_x=1501.718169    Next_y=245.050873    Next_angle=2910.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=245.050873   Current_angle=2910.000000\n",
      "4\n",
      "Next_x=1501.718169    Next_y=245.050873    Next_angle=2970.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=245.050873   Current_angle=2970.000000\n",
      "0\n",
      "Next_x=1501.718169    Next_y=256.610873    Next_angle=2970.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=256.610873   Current_angle=2970.000000\n",
      "4\n",
      "Next_x=1501.718169    Next_y=256.610873    Next_angle=3030.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=256.610873   Current_angle=3030.000000\n",
      "4\n",
      "Next_x=1501.718169    Next_y=256.610873    Next_angle=3090.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=256.610873   Current_angle=3090.000000\n",
      "0\n",
      "Next_x=1491.706915    Next_y=250.830873    Next_angle=3090.000000\n",
      "**********************************************************\n",
      "Current_x=1491.706915    Current_y=250.830873   Current_angle=3090.000000\n",
      "4\n",
      "Next_x=1491.706915    Next_y=250.830873    Next_angle=3150.000000\n",
      "**********************************************************\n",
      "Current_x=1491.706915    Current_y=250.830873   Current_angle=3150.000000\n",
      "4\n",
      "Next_x=1491.706915    Next_y=250.830873    Next_angle=3210.000000\n",
      "**********************************************************\n",
      "Current_x=1491.706915    Current_y=250.830873   Current_angle=3210.000000\n",
      "0\n",
      "Next_x=1501.718169    Next_y=245.050873    Next_angle=3210.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=245.050873   Current_angle=3210.000000\n",
      "4\n",
      "Next_x=1501.718169    Next_y=245.050873    Next_angle=3270.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=245.050873   Current_angle=3270.000000\n",
      "4\n",
      "Next_x=1501.718169    Next_y=245.050873    Next_angle=3330.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=245.050873   Current_angle=3330.000000\n",
      "0\n",
      "Next_x=1501.718169    Next_y=256.610873    Next_angle=3330.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=256.610873   Current_angle=3330.000000\n",
      "4\n",
      "Next_x=1501.718169    Next_y=256.610873    Next_angle=3390.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=256.610873   Current_angle=3390.000000\n",
      "4\n",
      "Next_x=1501.718169    Next_y=256.610873    Next_angle=3450.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=256.610873   Current_angle=3450.000000\n",
      "0\n",
      "Next_x=1491.706915    Next_y=250.830873    Next_angle=3450.000000\n",
      "**********************************************************\n",
      "Current_x=1491.706915    Current_y=250.830873   Current_angle=3450.000000\n",
      "4\n",
      "Next_x=1491.706915    Next_y=250.830873    Next_angle=3510.000000\n",
      "**********************************************************\n",
      "Current_x=1491.706915    Current_y=250.830873   Current_angle=3510.000000\n",
      "4\n",
      "Next_x=1491.706915    Next_y=250.830873    Next_angle=3570.000000\n",
      "**********************************************************\n",
      "Current_x=1491.706915    Current_y=250.830873   Current_angle=3570.000000\n",
      "0\n",
      "Next_x=1501.718169    Next_y=245.050873    Next_angle=3570.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=245.050873   Current_angle=3570.000000\n",
      "4\n",
      "Next_x=1501.718169    Next_y=245.050873    Next_angle=3630.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=245.050873   Current_angle=3630.000000\n",
      "4\n",
      "Next_x=1501.718169    Next_y=245.050873    Next_angle=3690.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=245.050873   Current_angle=3690.000000\n",
      "0\n",
      "Next_x=1501.718169    Next_y=256.610873    Next_angle=3690.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=256.610873   Current_angle=3690.000000\n",
      "4\n",
      "Next_x=1501.718169    Next_y=256.610873    Next_angle=3750.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=256.610873   Current_angle=3750.000000\n",
      "4\n",
      "Next_x=1501.718169    Next_y=256.610873    Next_angle=3810.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=256.610873   Current_angle=3810.000000\n",
      "0\n",
      "Next_x=1491.706915    Next_y=250.830873    Next_angle=3810.000000\n",
      "**********************************************************\n",
      "Current_x=1491.706915    Current_y=250.830873   Current_angle=3810.000000\n",
      "4\n",
      "Next_x=1491.706915    Next_y=250.830873    Next_angle=3870.000000\n",
      "**********************************************************\n",
      "Current_x=1491.706915    Current_y=250.830873   Current_angle=3870.000000\n",
      "4\n",
      "Next_x=1491.706915    Next_y=250.830873    Next_angle=3930.000000\n",
      "**********************************************************\n",
      "Current_x=1491.706915    Current_y=250.830873   Current_angle=3930.000000\n",
      "0\n",
      "Next_x=1501.718169    Next_y=245.050873    Next_angle=3930.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=245.050873   Current_angle=3930.000000\n",
      "4\n",
      "Next_x=1501.718169    Next_y=245.050873    Next_angle=3990.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=245.050873   Current_angle=3990.000000\n",
      "4\n",
      "Next_x=1501.718169    Next_y=245.050873    Next_angle=4050.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=245.050873   Current_angle=4050.000000\n",
      "0\n",
      "Next_x=1501.718169    Next_y=256.610873    Next_angle=4050.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=256.610873   Current_angle=4050.000000\n",
      "4\n",
      "Next_x=1501.718169    Next_y=256.610873    Next_angle=4110.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=256.610873   Current_angle=4110.000000\n",
      "4\n",
      "Next_x=1501.718169    Next_y=256.610873    Next_angle=4170.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=256.610873   Current_angle=4170.000000\n",
      "0\n",
      "Next_x=1491.706915    Next_y=250.830873    Next_angle=4170.000000\n",
      "**********************************************************\n",
      "Current_x=1491.706915    Current_y=250.830873   Current_angle=4170.000000\n",
      "4\n",
      "Next_x=1491.706915    Next_y=250.830873    Next_angle=4230.000000\n",
      "**********************************************************\n",
      "Current_x=1491.706915    Current_y=250.830873   Current_angle=4230.000000\n",
      "4\n",
      "Next_x=1491.706915    Next_y=250.830873    Next_angle=4290.000000\n",
      "**********************************************************\n",
      "Current_x=1491.706915    Current_y=250.830873   Current_angle=4290.000000\n",
      "0\n",
      "Next_x=1501.718169    Next_y=245.050873    Next_angle=4290.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=245.050873   Current_angle=4290.000000\n",
      "4\n",
      "Next_x=1501.718169    Next_y=245.050873    Next_angle=4350.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=245.050873   Current_angle=4350.000000\n",
      "4\n",
      "Next_x=1501.718169    Next_y=245.050873    Next_angle=4410.000000\n",
      "**********************************************************\n",
      "Current_x=1501.718169    Current_y=245.050873   Current_angle=4410.000000\n",
      "0\n",
      "Next_x=1501.718169    Next_y=256.610873    Next_angle=4410.000000\n"
     ]
    }
   ],
   "source": [
    " def Run():   \n",
    "    Destination_x,Destination_y=Set_destination(1500,250)\n",
    "    Current_x,Current_y,Current_angle=Start_point(500,1500,30)\n",
    "    \n",
    "    im=Draw_map_test(Destination_x,Destination_y)\n",
    "    Arrive=False\n",
    "    Crash=False\n",
    "    global Vec_x_tripod\n",
    "    global Vec_y_tripod\n",
    "    global Vec_x_quadruped\n",
    "    global Vec_y_quadruped\n",
    "    global Vec_x_onebyone\n",
    "    global Vec_y_onebyone\n",
    "    global Vec_x_turn\n",
    "    global Vec_y_turn\n",
    "    global Vec_time_tripod\n",
    "    global Vec_time_quadruped\n",
    "    global Vec_time_onebyone\n",
    "    global Vec_time_turn\n",
    "    Vec_x_tripod=[]\n",
    "    Vec_y_tripod=[]\n",
    "    Vec_x_quadruped=[]\n",
    "    Vec_y_quadruped=[]\n",
    "    Vec_x_onebyone=[]\n",
    "    Vec_y_onebyone=[]\n",
    "    Vec_x_turn=[]\n",
    "    Vec_y_turn=[]\n",
    "    Total_step=0\n",
    "    Vec_time_tripod=[]\n",
    "    Vec_time_quadruped=[]\n",
    "    Vec_time_onebyone=[]\n",
    "    Vec_time_turn=[]\n",
    "    \n",
    "    while Arrive!=True and Total_step<600:\n",
    "        Current_left_obstacle_level,_=Direction_min_level_1(Left_D,Current_x,Current_y,Current_angle,im)\n",
    "        Current_left_obstacle_level,_=Direction_min_level_1(Right_D,Current_x,Current_y,Current_angle,im)\n",
    "        Current_up_Obstacle_level,C=Direction_min_level_1(Up_D,Current_x,Current_y,Current_angle,im)\n",
    "        Current_state_avoid=Output_state_index_1(Current_left_obstacle_level,Current_left_obstacle_level\n",
    "                                                 ,Current_up_Obstacle_level)\n",
    "        \n",
    "        Avoid_action=int(Q_table1_gait_show.iloc[Current_state_avoid][4])\n",
    "        \n",
    "        True_angle,Left_right,Distance=Robot_destination_2(Destination_x,Destination_y,Current_x,Current_y,Current_angle)\n",
    "        Height_level=Height_level_2(Current_x,Current_y,Current_angle,im)\n",
    "        Current_state_arrive,Distance_level,Angle_level=Output_state_index_2(True_angle,Left_right,Distance,Height_level)\n",
    "        \n",
    "        Arrive_action=int(Q_table2_gait.iloc[Current_state_arrive][17])\n",
    "        \n",
    "        Next_x,Next_y,Next_angle,Next_action=Which_table_action(Avoid_action,Arrive_action,Current_x,Current_y,Current_angle,Destination_x,Destination_y,im)\n",
    "        \n",
    "        Arrive=Is_arrive_2(Next_x,Next_y,im)\n",
    "        \n",
    "        print('**********************************************************')\n",
    "        print('Current_x=%f    Current_y=%f   Current_angle=%f'%(Current_x,Current_y,Current_angle))\n",
    "        print(Next_action)\n",
    "        print('Next_x=%f    Next_y=%f    Next_angle=%f'%(Next_x,Next_y,Next_angle))\n",
    "#         print('DestinationRobotAngle=%f Robot_current_angle=%d'%((np.arctan((Next_y-Destination_y)/(Next_x-Destination_x)))*180/np.pi,Next_angle%360))\n",
    "        \n",
    "        if Arrive==True:\n",
    "            if Next_action==0:\n",
    "                Vec_x_tripod.append(Current_x)\n",
    "                Vec_y_tripod.append(Current_y)\n",
    "                Vec_time_tripod.append(Total_step)\n",
    "            elif Next_action==1:\n",
    "                Vec_x_quadruped.append(Current_x)\n",
    "                Vec_y_quadruped.append(Current_y)\n",
    "                Vec_time_quadruped.append(Total_step)\n",
    "            elif Next_action==2:\n",
    "                Vec_x_onebyone.append(Current_x)\n",
    "                Vec_y_onebyone.append(Current_y)\n",
    "                Vec_time_onebyone.append(Total_step)\n",
    "            else:\n",
    "                Vec_x_turn.append(Current_x)\n",
    "                Vec_y_turn.append(Current_y) \n",
    "                Vec_time_turn.append(Total_step)\n",
    "\n",
    "            break\n",
    "        else:\n",
    "            if Next_action==0:\n",
    "                Vec_x_tripod.append(Current_x)\n",
    "                Vec_y_tripod.append(Current_y)\n",
    "                Vec_time_tripod.append(Total_step)\n",
    "            elif Next_action==1:\n",
    "                Vec_x_quadruped.append(Current_x)\n",
    "                Vec_y_quadruped.append(Current_y)\n",
    "                Vec_time_quadruped.append(Total_step)\n",
    "            elif Next_action==2:\n",
    "                Vec_x_onebyone.append(Current_x)\n",
    "                Vec_y_onebyone.append(Current_y)\n",
    "                Vec_time_onebyone.append(Total_step)\n",
    "            else:\n",
    "                Vec_x_turn.append(Current_x)\n",
    "                Vec_y_turn.append(Current_y)\n",
    "                Vec_time_turn.append(Total_step)\n",
    "            Current_x=Next_x\n",
    "            Current_y=Next_y\n",
    "            Current_angle=Next_angle\n",
    "            Total_step+=1\n",
    "Run()\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 55,
   "metadata": {},
   "outputs": [],
   "source": [
    "im_movement=Movement_plot(Vec_x_tripod,Vec_y_tripod,Vec_x_quadruped,Vec_y_quadruped,Vec_x_onebyone,Vec_y_onebyone,\n",
    "                  Vec_x_turn,Vec_y_turn,1500,250)\n",
    "plt.imshow(im_movement)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 56,
   "metadata": {},
   "outputs": [
    {
     "ename": "NameError",
     "evalue": "name 'Vec_time' is not defined",
     "output_type": "error",
     "traceback": [
      "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[1;31mNameError\u001b[0m                                 Traceback (most recent call last)",
      "\u001b[1;32m<ipython-input-56-188b69053807>\u001b[0m in \u001b[0;36m<module>\u001b[1;34m()\u001b[0m\n\u001b[1;32m----> 1\u001b[1;33m \u001b[0mVec_time\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0m",
      "\u001b[1;31mNameError\u001b[0m: name 'Vec_time' is not defined"
     ]
    }
   ],
   "source": [
    "Vec_time"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
